Diff: Spektrum.cpp

Revision:

0:f4edd3407cc5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Spektrum.cpp	Thu Jan 17 00:28:07 2019 +0000
@@ -0,0 +1,166 @@
+/*
+  Spektrum.cpp
+  Implementation file for Spektrum serial receiver mbed library
+  Dennis Evangelista, 2018
+*/
+
+#include "mbed.h"
+#include "rtos.h"
+
+#include "Spektrum.h"
+
+
+
+
+
+
+/** Spektrum is used for a working connection to a Spektrum Satellite receiver
+    @param tx orange wire, 3.3V supply pin (should be held high normally)
+    @param rx gray wire, rx pin from the receiver
+    The black wire should be connected to ground. 
+    The receiver should first be bound using a BindPlug object. 
+*/
+Spektrum::Spektrum(PinName tx, PinName rx):
+  _rx(tx, rx, SPEKTRUM_BAUD){
+  
+  // receiver uses e.g. p13, p14 and 115200 baud. 
+  _rx.set_blocking(false); // want receiver to not block
+  period_ms = 11; // start with 11ms period.
+
+  // initialize public variables... 
+  system = 0; // should be 0xa2 or 0xb2 for DSMX modes
+  fades = 0;
+  for (int i=0; i<SPEKTRUM_CHANNELS; i++){
+    channel[i] = 0; 
+    pulsewidth[i] = 0;
+  }
+  valid = false; 
+
+  // start the packet reading thread
+  _packet_thread.start(callback(this,&Spektrum::_packet_callback)); 
+} // Spektrum(tx, rx) constructor
+
+
+
+
+/** Destructor for Spektrum object 
+ */
+Spektrum::~Spektrum(){
+} // ~Spektrum() destructor
+
+
+
+/** Private callback for when a packet is received
+ */
+void Spektrum::_packet_callback(void){
+  // local variables
+  int count; // used to get error code -11 or num of bytes _rx.read()
+  uint64_t now; // for getting 11 or 22 ms period via ThisThread::sleep_until()
+  unsigned int i; // counter for for loop
+  unsigned int data; // for assembling 2 bytes into uint16_t
+  unsigned int chanid; // for decoding channel ID with mask 0x7800
+  unsigned int servopos; // for decoding servo value with mask 0x07ff
+  
+  // setup
+  //debug("Spektrum::_packet_thread started\r\n");
+
+  // loop
+  _rx.sync(); // flush buffer
+  while(1){
+    now = rtos::Kernel::get_ms_count(); // for timing
+    count = _rx.read(_buf,SPEKTRUM_PACKET_SIZE); // try to read packet
+    
+    if (count == SPEKTRUM_PACKET_SIZE){
+      // got a full sized packet
+      if (_buf[1] == SPEKTRUM_22MS_2048_DSMX){
+    period_ms = 22; 
+    valid = true;
+      } // got 22ms packets
+      else if (_buf[1] == SPEKTRUM_11MS_2048_DSMX){
+    period_ms = 11;
+    valid = true;
+      } // got 11ms packets
+      else
+    // if system is not 0xa2 or 0xb2, treat as invalid
+    valid = false;
+    } // if count == 16
+    else {
+      // count wasn't 16 so some kind of error
+      valid = false;
+      _rx.sync(); // not getting enough bytes, so sync()
+    }
+
+    if (valid){
+      // got a valid packet so parse it 
+      fades = _buf[0];
+      system = _buf[1];
+      for (i=0; i<SPEKTRUM_SERVOS; i++){
+    data = (_buf[2*i+2]<<8) | _buf[2*i+2+1];
+    chanid = (data & SPEKTRUM_MASK_2048_CHANID) >> 11;
+    servopos = data & SPEKTRUM_MASK_2048_SXPOS;
+    channel[chanid] = servopos;
+    pulsewidth[chanid] = SPEKTRUM_COUNT2US(servopos); 
+      } // for each servo in packet
+    } // if(valid)
+
+    ThisThread::sleep_until(now+period_ms); // sleep to get right rate
+  } // while(1)
+} // _packet_callback() 
+
+
+
+
+
+
+
+
+
+/** BindPlug is used to bind a Spektrum Satellite receiver
+    @param tx orange wire, 3.3V supply pin, here used as a DigitalOut
+    @param rx gray wire, rx pin, here used as a DigitalOut
+    @param mode is mode, e.g. internal or external, DSM2 or DSMX, 11 or 22ms
+    The black wire should be connected to ground.
+    Default mode is internal, DSMX, 11 ms. Once created, this object will 
+    send a number of falling pulses over the 3.3V supply pin to trigger 
+    the satellite receiver to go into bind mode. 
+*/
+BindPlug::BindPlug(PinName tx, PinName rx, int mode): _3Vpin(tx),_datapin(rx){
+  int i; // counter
+  
+  // within 200 ms of applying power, supply a bunch of falling pulses
+  // according to table in Spektrum docs, most likely 9 pulses for
+  // internal mode, DSMX, 11 ms.
+  _3Vpin = 0;
+  _datapin = 0; 
+  wait(1);
+  _3Vpin = 1; 
+  _datapin = 1;
+  wait_ms(72);
+  debug("pulse ");
+  for(i=0; i<mode; i++){
+    debug("%d ",i);
+    wait_us(116);
+    _datapin = 0; // this is the falling pulse
+    wait_us(116);
+    _datapin = 1; 
+  }
+  debug("\r\n"); 
+} // BindPlug(bind, mode) constructor
+
+/** Destructor for BindPlug object
+ */
+BindPlug::~BindPlug(){
+} // ~BindPlug() destructor
+
+
+
+
+
+
+/* LATER
+SpektrumTestDevice::SpektrumTestDevice(PinName tx, PinName rx): _receiver(tx,rx){
+  _receiver.baud(SPEKTRUM_BAUD);
+} // SpektrumTestDevice(tx, rx) constructor
+SpektrumTestDevice::~SpektrumTestDevice(){
+} // ~SpektrumTestDevice() destructor
+*/
\ No newline at end of file