3/18/19
Dependencies: mbed BNO055 MBed_Adafruit-GPS-Library
Spektrum.cpp
- Committer:
- LukeMar
- Date:
- 2019-01-17
- Revision:
- 0:f4edd3407cc5
File content as of revision 0:f4edd3407cc5:
/* 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 */