Matthew Marino
/
sailbot_CONTTHREAD
Hopefully Drew can find this
Revision 0:8ed926c84e2f, committed 2018-11-28
- Comitter:
- LukeMar
- Date:
- Wed Nov 28 14:53:46 2018 +0000
- Commit message:
- try
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Spektrum.cpp Wed Nov 28 14:53:46 2018 +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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Spektrum.h Wed Nov 28 14:53:46 2018 +0000 @@ -0,0 +1,115 @@ +/* + Spektrum.h + Spektrum serial receiver mbed library + Dennis Evangelista, 2018 +*/ + +#ifndef SPEKTRUM_H +#define SPEKTRUM_H +#define SPEKTRUM_VERSION "1.0.1" + +#include "mbed.h" +#include "rtos.h" + +// bind modes +#define SPEKTRUM_INT_DSMX_22MS 7 +#define SPEKTRUM_EXT_DSMX_22MS 8 +#define SPEKTRUM_INT_DSMX_11MS 9 +#define SPEKTRUM_EXT_DSMX_11MS 10 +// DSM2 bind modes not recommended, not implemented +// EXT(ernal) bind modes don't really work on solitary satellite receiver + +// field definitions +// #define SPEKTRUM_MASK_1024_CHANID 0xfc00 +// #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 +// #define SPEKTRUM_22MS_1024_DSM2 0x01 +// #define SPEKTRUM_11MS_2048_DSM2 0x12 +#define SPEKTRUM_22MS_2048_DSMX 0xa2 +#define SPEKTRUM_11MS_2048_DSMX 0xb2 + +#define SPEKTRUM_BAUD 115200 +// Spektrum baud is supposed to be 125000, but the LPC1768 seems not +// to support nonstandard baud rates. + +#define SPEKTRUM_SERVOS 7 +#define SPEKTRUM_PACKET_SIZE 16 +#define SPEKTRUM_CHANNELS 16 +#define SPEKTRUM_COUNT2US(x) (x*600/1024+900) + + + + +/** Spektrum receiver object for connecting to eg SPM9745 receiver + */ +class Spektrum +{ +public: + /** Number of fades (failed packets) from receiver */ + unsigned int fades; + + /** Tells if system is in DSMX 11ms or 22ms for example */ + unsigned int system; + + /** Contains 0-2048 values for all channels 0-15 */ + unsigned int channel[SPEKTRUM_CHANNELS]; + + /** Contains approx 900-2100us pulsewidths corresponding to chan 0-15 */ + unsigned int pulsewidth[SPEKTRUM_CHANNELS]; + + /** If true, data is value */ + bool valid; // TODO switch to EventFlags? + + /** 11 or 22 ms */ + unsigned int period_ms; + + Spektrum(PinName tx, PinName rx); // constructor + ~Spektrum(); // destructor + +private: + UARTSerial _rx; + unsigned char _buf[SPEKTRUM_PACKET_SIZE]; + Thread _packet_thread; + void _packet_callback(void); +}; + + + + + + + +/** For binding a Spektrum receiver to transmitter */ +class BindPlug +{ +public: + /** e.g. SPEKTRUM_INT_DSMX_11MS, SPEKTRUM_INT_DSMX_22MS */ + int mode; + + BindPlug(PinName tx, PinName rx, int mode = SPEKTRUM_INT_DSMX_11MS); + ~BindPlug(); + void bind(); + +private: + DigitalOut _3Vpin; + DigitalOut _datapin; +}; + +/* LATER +class SpektrumTestDevice{ + public: + unsigned int fades; + unsigned int servo[7]; + SpektrumTestDevice(PinName tx, PinName rx); + ~SpektrumTestDevice(); + private: + Serial _receiver; +}; +*/ + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Nov 28 14:53:46 2018 +0000 @@ -0,0 +1,109 @@ +/* + Test for manual control thread + Spektrum receiver simple test + + range on transmitter is 344-1564 + to correspond to rudder angle of 90-270 + current eqn to convert RC output is + + (IN/6.77)+39.19 + + NB: The specification for Spektrum Remote Receiver Interfacing, + rev A, 12 Apr 2016, has a note about servo position ranges: + "A full range of servo position data ranges from 0-1024 or 0-2045 depending + on the bind type. These limits are equivalent to a +/-150% travel setting + in AirWare. This full range translates to a range of 1194us which when + applied to a pwm servo signal equals 903-2097us. At +/-100% travel, the + data range is equivalent to a servo position data range of approximately + 341 to 1707 which translated to pwm equals 1102 us to 1898 us." +*/ + +#include "mbed.h" +//#include "rtos.h" +#include "Spektrum.h" +#include "unity.h" +//****RC control universal variables**** +Serial pc(USBTX, USBRX, 115200); +Spektrum rx(p13, p14); +char c; +int i; +int j; +float now; +//***Stepper Motor universal variables*** +AnalogIn ain(p18); +PwmOut motor( p22 ); +DigitalOut dir( p21 ); +DigitalOut I(p23); +DigitalOut slp(p30); //sleep +float d_ang = 180.0; +float pos = 180.0; +float RC_O; + +//**get position** +float posi(float in); + + +int main(void) +{ + + I=0; + motor.period(.001); + motor.pulsewidth(0); + // loop + while(1) { + //now = rtos::Kernel::get_ms_count(); + //***JUST ADDED stepper code + + if (rx.valid) { + RC_O = rx.channel[0]; + pc.printf(" rc: %f\n",rx.channel[0]); + } else { + pc.printf(" invalid\r\n"); + slp = 0; + } + //39.19 + d_ang = (RC_O/6.77)+39.19; + + pos = (ain-.108)/.002466; + //pc.printf(" %.3f\n",); + if((pos > (d_ang-3.0)) && (pos < (d_ang+3.0))) { + motor.pulsewidth(0); + slp = 0; + } + if( (pos > (d_ang+3.0)) && pos < 270.0) { + slp = 1; + while(pos > d_ang) { + dir = 1; //left?? + motor.pulsewidth(.0005); + pc.printf("pos: %.3f\n",pos); + pos = posi(ain); + }//while pos + motor.pulsewidth(0); + }//if pos + if((pos < (d_ang-3.0)) && pos > 90.0) { + slp = 1; + while(pos < d_ang) { + dir = 0; //right?? + motor.pulsewidth(.0005); + pc.printf("pos: %.3f\n",pos); + pos = posi(ain); + }//while pos + motor.pulsewidth(0); + } + //***END OF JUST ADDED + ThisThread::sleep_until(121); + }//while(1) +} // main() for TESTS/receiver/simple + + + +float posi(float in) +{ + float p1; + float p2; + float p3; + p1 = (ain-.108)/.002466; + p2 = (ain-.108)/.002466; + p3 = (ain-.108)/.002466; + return (p1+p2+p3)/3.0; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Wed Nov 28 14:53:46 2018 +0000 @@ -0,0 +1,1 @@ +https://github.com/armmbed/mbed-os/#2fd0c5cfbd83fce62da6308f9d64c0ab64e1f0d6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 28 14:53:46 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file