[paparazzi-commits] [5486] refactored spektrum parser

[Eric]

Revision: 5486
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5486
Author:   lamestllama
Date:     2010-08-19 15:36:50 +0000 (Thu, 19 Aug 2010)
Log Message:
-----------
refactored spektrum parser

Modified Paths:
--------------
    
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h

Modified: 
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h
       2010-08-19 15:36:47 UTC (rev 5485)
+++ 
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h
       2010-08-19 15:36:50 UTC (rev 5486)
@@ -1,7 +1,7 @@
 /*
- * $Id$
+ * Paparazzi $Id$
  *  
- * Copyright (C) 2009 Eric Parsonage <address@hidden>
+ * Copyright (C) 2009-2010 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -18,14 +18,85 @@
  * 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. 
+ * Boston, MA 02111-1307, USA.
+ *
  */
 
-/*
- *Function to bind Spektrum satellite receivers
- */
-
 #ifndef BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H
 #define BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H
-extern void radio_control_spektrum_try_bind( void );
-#endif  /* BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H */
+
+#include "std.h"
+#include "uart.h"
+
+#include RADIO_CONTROL_SPEKTRUM_MODEL_H
+
+#define RC_SPK_SYNC_1 0x03
+
+#define RC_SPK_STA_UNINIT     0
+#define RC_SPK_STA_GOT_SYNC_1 1
+#define RC_SPK_STA_GOT_SYNC_2 2
+
+extern bool_t   rc_spk_parser_status;
+extern uint8_t  rc_spk_parser_idx;
+extern uint8_t  rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL*2];
+
+#define MAX_SPK 344 
+
+
+extern const int16_t rc_spk_throw[RADIO_CONTROL_NB_CHANNEL];
+
+#define __RcLink(dev, _x) dev##_x
+#define _RcLink(dev, _x)  __RcLink(dev, _x)
+#define RcLink(_x) _RcLink(RADIO_CONTROL_LINK, _x)
+
+#define RcLinkChAvailable() RcLink(ChAvailable())
+#define RcLinkGetCh() RcLink(Getch())
+
+#define _RadioControlEvent(_received_frame_handler) {                    \
+    while (RcLinkChAvailable()) {                                       \
+      int8_t c = RcLinkGetCh();                                         \
+      switch (rc_spk_parser_status) {                                   \
+      case RC_SPK_STA_UNINIT:                                           \
+        if (c==RC_SPK_SYNC_1)                                           \
+          rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_1;                 \
+        break;                                                          \
+      case RC_SPK_STA_GOT_SYNC_1:                                       \
+        if (c==RC_SPK_SYNC_2) {                                         \
+          rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_2;                 \
+          rc_spk_parser_idx = 0;                                        \
+        }                                                               \
+        else                                                            \
+          rc_spk_parser_status = RC_SPK_STA_UNINIT;                     \
+        break;                                                          \
+      case RC_SPK_STA_GOT_SYNC_2:                                       \
+        rc_spk_parser_buf[rc_spk_parser_idx] = c;                       \
+        rc_spk_parser_idx++;                                            \
+        if (rc_spk_parser_idx >= 2*RADIO_CONTROL_NB_CHANNEL) {          \
+          rc_spk_parser_status = RC_SPK_STA_UNINIT;                     \
+          radio_control.frame_cpt++;                                   \
+          radio_control.time_since_last_frame = 0;                     \
+          radio_control.status = RADIO_CONTROL_OK;                     \
+         uint8_t i;                                                    \
+         for (i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) {                    \
+           const int16_t tmp = (rc_spk_parser_buf[2*i]<<8) +           \
+                                rc_spk_parser_buf[2*i+1];              \
+           /*const int16_t chan = (tmp&0xFC00) >> 10;*/                \
+           const int16_t val  = (tmp&0x03FF) - 512;                    \
+           radio_control.values[i] = val;                              \
+           radio_control.values[i] *= rc_spk_throw[i];                 \
+           if (i==RADIO_CONTROL_THROTTLE) {                            \
+             radio_control.values[i] += MAX_PPRZ;                      \
+             radio_control.values[i] /= 2;                             \
+           }                                                           \
+         }                                                             \
+         _received_frame_handler();                                    \
+        }                                                               \
+        break;                                                          \
+      default:                                                         \
+       rc_spk_parser_status = RC_SPK_STA_UNINIT;                       \
+      }                                                                 \
+    }                                                                   \
+  }
+
+
+#endif /* BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H */