3/18/19
Dependencies: mbed BNO055 MBed_Adafruit-GPS-Library
Diff: Spektrum.cpp
- Revision:
- 0:f4edd3407cc5
diff -r 000000000000 -r f4edd3407cc5 Spektrum.cpp --- /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