From e63826d9ee0a58249cabf483cc0663d221e022d6 Mon Sep 17 00:00:00 2001
From: Dennis Evangelista <evangeli@usna.edu>
Date: Wed, 14 Nov 2018 16:42:39 -0500
Subject: [PATCH] Added Spektrum packet stuff

---
 TESTS/receiver/simple/main.cpp | 16 ++++++++--
 spektrum.cpp                   | 56 ++++++++++++++++++++++++++++++----
 spektrum.h                     | 20 +++++++-----
 3 files changed, 76 insertions(+), 16 deletions(-)

diff --git a/TESTS/receiver/simple/main.cpp b/TESTS/receiver/simple/main.cpp
index 15cfc28..a68d474 100644
--- a/TESTS/receiver/simple/main.cpp
+++ b/TESTS/receiver/simple/main.cpp
@@ -12,14 +12,24 @@
 Serial pc(USBTX, USBRX);
 Spektrum receiver(p13, p14); 
 char c;
+int i; 
 
 
-
-int main(){
+int main(void){
 
   pc.printf("Spektrum receiver simple test\r\n");
   pc.printf("Be sure to connect orange to p13, gray to p14, blk to gnd\r\n");
-  pc.printf("Receiver should have already been bound to transmitter\r\n");
+  pc.printf("Receiver should have already been bound to transmitter DSMX 22ms\r\n");
+  pc.printf("Echoing about 22s of data now...\r\n");
+
+  for (i=0; i<1000; i++){
+    pc.printf("%d. system %x, fades %d, channel %04d %04d %04d %04d %04d %04d %04d %04d %04d\r\n",
+	      i, receiver.system, receiver.fades,
+	      receiver.channel[0], receiver.channel[1], receiver.channel[2],
+	      receiver.channel[3], receiver.channel[4], receiver.channel[5],
+	      receiver.channel[6], receiver.channel[7], receiver.channel[8]);
+    ThisThread::sleep_for(22); 
+  }//for
   
   pc.printf("Successful (y/n)? ");
   pc.scanf(" %c",&c);
diff --git a/spektrum.cpp b/spektrum.cpp
index ae16795..6f3c1b7 100644
--- a/spektrum.cpp
+++ b/spektrum.cpp
@@ -13,17 +13,59 @@
 Spektrum::Spektrum(PinName tx, PinName rx, PinName rx_led): _receiver(tx, rx), _rx_led(rx_led){
   _receiver.baud(SPEKTRUM_BAUD); // Spektrum uses 125000 8N1 or 115200 8N1
 
-  _receiver.attach(callback(this,&Spektrum::_rx_callback)); 
+  _rx_thread.start(callback(this,&Spektrum::_rx_callback)); 
 } // Spektrum(tx, rx) constructor
 
 Spektrum::~Spektrum(){
 } // ~Spektrum() destructor
 
 void Spektrum::_rx_callback(void){
-  _rx_led.write(!_rx_led.read());
-  _buf[_i] = _receiver.getc();
-  _i = (_i+1) % SPEKTRUM_PACKET_SIZE;
-} // _packet_callback() 
+  unsigned char c;        // character read 
+  unsigned int i;         // for loop counter
+  unsigned int channelid; // for unpacking channel id 
+  unsigned int servopos;  // for unpacking servo position data
+  
+  // setup
+  debug("Spektrum rx_thread started\r\n");
+
+  // loop
+  while(1){
+    if (_receiver.readable()){
+	c = _receiver.getc();
+	switch(_state){
+
+	case 0: // idle, waiting for start
+	  if ((c==SPEKTRUM_22MS_2048_DSMX) || (c==SPEKTRUM_11MS_2048_DSMX))
+	    system = c; 
+	    _state = 1;
+	  break;
+
+	case 1: // got 0xa2 or 0xb2, get fades next
+	  fades = c;
+	  _state = 2;
+	  break;
+
+	default:
+	  _data[_state-2] = c;
+	  _state++;
+	  if (_state == SPEKTRUM_NUM_BYTES_IN_FRAME-1){
+
+	    for (i=0; i<SPEKTRUM_SERVOS; i++){
+	      channelid = (_data[2*i] & SPEKTRUM_MASK_2048_CHANID_MSB) >> 3;
+	      servopos = ((_data[2*i] << 8) | _data[2*i+1]) & SPEKTRUM_MASK_2048_SXPOS;
+	      if ((channelid >= 0) && (channelid < SPEKTRUM_CHANNELS))
+		channel[channelid] = servopos;
+	    } // unpack data into channels
+
+	    // output data is now valid
+	    // set some flags LATER
+
+	    _state = 0; // reset state machine to idle
+	  }// if complete packet received
+	} // switch(_state)
+    } // if readable
+  } // while(1)
+} // _rx_callback() 
 
 
 
@@ -70,10 +112,12 @@ BindPlug::~BindPlug(){
 
 
 
-
+/*
+// LATER
 SpektrumTestDevice::SpektrumTestDevice(PinName tx, PinName rx): _receiver(tx,rx){
   _receiver.baud(SPEKTRUM_BAUD);
 } // SpektrumTestDevice(tx, rx) constructor
 
 SpektrumTestDevice::~SpektrumTestDevice(){
 } // ~SpektrumTestDevice() destructor
+*/
diff --git a/spektrum.h b/spektrum.h
index 18ef8f5..8e54eaf 100644
--- a/spektrum.h
+++ b/spektrum.h
@@ -17,6 +17,7 @@
 // #define SPEKTRUM_MASK_1024_SXPOS 0x03ff
 // first two only used in DSM2 which are not implemented
 #define SPEKTRUM_MASK_2048_CHANID 0x7800
+#define SPEKTRUM_MASK_2048_CHANID_MSB 0x78
 #define SPEKTRUM_MASK_2048_SXPOS 0x07ff
 
 // allowable system field values
@@ -28,22 +29,26 @@
 #define SPEKTRUM_BAUD 125000
 // Spektrum baud is 125000, but if this doesn't work 115200 should work too. 
 
-#define SPEKTRUM_PACKET_SIZE 16
+#define SPEKTRUM_SERVOS (7)
+#define SPEKTRUM_NUM_BYTES_IN_FRAME (2*SPEKTRUM_SERVOS+2)
+#define SPEKTRUM_NUM_BYTES_SERVOS (2*SPEKTRUM_SERVOS)
+#define SPEKTRUM_CHANNELS 12
 
 class Spektrum{
  public:
   unsigned int system; 
   unsigned int fades; 
-  unsigned int channel[12];
+  unsigned int channel[SPEKTRUM_CHANNELS];
   Spektrum(PinName tx, PinName rx, PinName rx_led=LED1); // constructor
   ~Spektrum(); // destructor
 
  private:
   Serial _receiver;
   DigitalOut _rx_led;
-  
-  char _buf[SPEKTRUM_PACKET_SIZE];
-  int _i; 
+
+  char _state; 
+  unsigned char _data[SPEKTRUM_NUM_BYTES_SERVOS];
+  Thread _rx_thread;
   void _rx_callback(void); 
 };
 
@@ -63,7 +68,8 @@ class BindPlug{
   DigitalOut _datapin;
 };
 
-
+/*
+// LATER
 class SpektrumTestDevice{
  public:
   unsigned int fades;
@@ -74,6 +80,6 @@ class SpektrumTestDevice{
  private:
   Serial _receiver; 
 };
-
+*/
 
 #endif