Spektrum.cpp

00001 /*
00002   Spektrum.cpp
00003   Implementation file for Spektrum serial receiver mbed library
00004   Dennis Evangelista, 2018
00005 */
00006 
00007 #include "mbed.h"
00008 #include "rtos.h"
00009 
00010 #include "Spektrum.h"
00011 
00012 
00013 
00014 
00015 
00016 
00017 /** Spektrum is used for a working connection to a Spektrum Satellite receiver
00018     @param tx orange wire, 3.3V supply pin (should be held high normally)
00019     @param rx gray wire, rx pin from the receiver
00020     The black wire should be connected to ground. 
00021     The receiver should first be bound using a BindPlug object. 
00022 */
00023 Spektrum::Spektrum(PinName tx, PinName rx):
00024   _rx(tx, rx, SPEKTRUM_BAUD){
00025   
00026   // receiver uses e.g. p13, p14 and 115200 baud. 
00027   _rx.set_blocking(false); // want receiver to not block
00028   period_ms = 11; // start with 11ms period.
00029 
00030   // initialize public variables... 
00031   system = 0; // should be 0xa2 or 0xb2 for DSMX modes
00032   fades = 0;
00033   for (int i=0; i<SPEKTRUM_CHANNELS; i++){
00034     channel[i] = 0; 
00035     pulsewidth[i] = 0;
00036   }
00037   valid = false; 
00038 
00039   // start the packet reading thread
00040   _packet_thread.start(callback(this,&Spektrum::_packet_callback)); 
00041 } // Spektrum(tx, rx) constructor
00042 
00043 
00044 
00045 
00046 /** Destructor for Spektrum object 
00047  */
00048 Spektrum::~Spektrum(){
00049 } // ~Spektrum() destructor
00050 
00051 
00052 
00053 /** Private callback for when a packet is received
00054  */
00055 void Spektrum::_packet_callback(void){
00056   // local variables
00057   int count; // used to get error code -11 or num of bytes _rx.read()
00058   uint64_t now; // for getting 11 or 22 ms period via ThisThread::sleep_until()
00059   unsigned int i; // counter for for loop
00060   unsigned int data; // for assembling 2 bytes into uint16_t
00061   unsigned int chanid; // for decoding channel ID with mask 0x7800
00062   unsigned int servopos; // for decoding servo value with mask 0x07ff
00063   
00064   // setup
00065   //debug("Spektrum::_packet_thread started\r\n");
00066 
00067   // loop
00068   _rx.sync(); // flush buffer
00069   while(1){
00070     now = rtos::Kernel::get_ms_count(); // for timing
00071     count = _rx.read(_buf,SPEKTRUM_PACKET_SIZE); // try to read packet
00072     
00073     if (count == SPEKTRUM_PACKET_SIZE){
00074       // got a full sized packet
00075       if (_buf[1] == SPEKTRUM_22MS_2048_DSMX){
00076     period_ms = 22; 
00077     valid = true;
00078       } // got 22ms packets
00079       else if (_buf[1] == SPEKTRUM_11MS_2048_DSMX){
00080     period_ms = 11;
00081     valid = true;
00082       } // got 11ms packets
00083       else
00084     // if system is not 0xa2 or 0xb2, treat as invalid
00085     valid = false;
00086     } // if count == 16
00087     else {
00088       // count wasn't 16 so some kind of error
00089       valid = false;
00090       _rx.sync(); // not getting enough bytes, so sync()
00091     }
00092 
00093     if (valid){
00094       // got a valid packet so parse it 
00095       fades = _buf[0];
00096       system = _buf[1];
00097       for (i=0; i<SPEKTRUM_SERVOS; i++){
00098     data = (_buf[2*i+2]<<8) | _buf[2*i+2+1];
00099     chanid = (data & SPEKTRUM_MASK_2048_CHANID) >> 11;
00100     servopos = data & SPEKTRUM_MASK_2048_SXPOS;
00101     channel[chanid] = servopos;
00102     pulsewidth[chanid] = SPEKTRUM_COUNT2US(servopos); 
00103       } // for each servo in packet
00104     } // if(valid)
00105 
00106     ThisThread::sleep_until(now+period_ms); // sleep to get right rate
00107   } // while(1)
00108 } // _packet_callback() 
00109 
00110 
00111 
00112 
00113 
00114 
00115 
00116 
00117 
00118 /** BindPlug is used to bind a Spektrum Satellite receiver
00119     @param tx orange wire, 3.3V supply pin, here used as a DigitalOut
00120     @param rx gray wire, rx pin, here used as a DigitalOut
00121     @param mode is mode, e.g. internal or external, DSM2 or DSMX, 11 or 22ms
00122     The black wire should be connected to ground.
00123     Default mode is internal, DSMX, 11 ms. Once created, this object will 
00124     send a number of falling pulses over the 3.3V supply pin to trigger 
00125     the satellite receiver to go into bind mode. 
00126 */
00127 BindPlug::BindPlug(PinName tx, PinName rx, int mode): _3Vpin(tx),_datapin(rx){
00128   int i; // counter
00129   
00130   // within 200 ms of applying power, supply a bunch of falling pulses
00131   // according to table in Spektrum docs, most likely 9 pulses for
00132   // internal mode, DSMX, 11 ms.
00133   _3Vpin = 0;
00134   _datapin = 0; 
00135   wait(1);
00136   _3Vpin = 1; 
00137   _datapin = 1;
00138   wait_ms(72);
00139   debug("pulse ");
00140   for(i=0; i<mode; i++){
00141     debug("%d ",i);
00142     wait_us(116);
00143     _datapin = 0; // this is the falling pulse
00144     wait_us(116);
00145     _datapin = 1; 
00146   }
00147   debug("\r\n"); 
00148 } // BindPlug(bind, mode) constructor
00149 
00150 /** Destructor for BindPlug object
00151  */
00152 BindPlug::~BindPlug(){
00153 } // ~BindPlug() destructor
00154 
00155 
00156 
00157 
00158 
00159 
00160 /* LATER
00161 SpektrumTestDevice::SpektrumTestDevice(PinName tx, PinName rx): _receiver(tx,rx){
00162   _receiver.baud(SPEKTRUM_BAUD);
00163 } // SpektrumTestDevice(tx, rx) constructor
00164 SpektrumTestDevice::~SpektrumTestDevice(){
00165 } // ~SpektrumTestDevice() destructor
00166 */