Matthew Marino
/
sailbot_RUDDER
Rudder program
Spektrum.cpp@0:8ed926c84e2f, 2018-11-28 (annotated)
- Committer:
- LukeMar
- Date:
- Wed Nov 28 14:53:46 2018 +0000
- Revision:
- 0:8ed926c84e2f
try
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LukeMar | 0:8ed926c84e2f | 1 | /* |
LukeMar | 0:8ed926c84e2f | 2 | Spektrum.cpp |
LukeMar | 0:8ed926c84e2f | 3 | Implementation file for Spektrum serial receiver mbed library |
LukeMar | 0:8ed926c84e2f | 4 | Dennis Evangelista, 2018 |
LukeMar | 0:8ed926c84e2f | 5 | */ |
LukeMar | 0:8ed926c84e2f | 6 | |
LukeMar | 0:8ed926c84e2f | 7 | #include "mbed.h" |
LukeMar | 0:8ed926c84e2f | 8 | #include "rtos.h" |
LukeMar | 0:8ed926c84e2f | 9 | |
LukeMar | 0:8ed926c84e2f | 10 | #include "Spektrum.h" |
LukeMar | 0:8ed926c84e2f | 11 | |
LukeMar | 0:8ed926c84e2f | 12 | |
LukeMar | 0:8ed926c84e2f | 13 | |
LukeMar | 0:8ed926c84e2f | 14 | |
LukeMar | 0:8ed926c84e2f | 15 | |
LukeMar | 0:8ed926c84e2f | 16 | |
LukeMar | 0:8ed926c84e2f | 17 | /** Spektrum is used for a working connection to a Spektrum Satellite receiver |
LukeMar | 0:8ed926c84e2f | 18 | @param tx orange wire, 3.3V supply pin (should be held high normally) |
LukeMar | 0:8ed926c84e2f | 19 | @param rx gray wire, rx pin from the receiver |
LukeMar | 0:8ed926c84e2f | 20 | The black wire should be connected to ground. |
LukeMar | 0:8ed926c84e2f | 21 | The receiver should first be bound using a BindPlug object. |
LukeMar | 0:8ed926c84e2f | 22 | */ |
LukeMar | 0:8ed926c84e2f | 23 | Spektrum::Spektrum(PinName tx, PinName rx): |
LukeMar | 0:8ed926c84e2f | 24 | _rx(tx, rx, SPEKTRUM_BAUD){ |
LukeMar | 0:8ed926c84e2f | 25 | |
LukeMar | 0:8ed926c84e2f | 26 | // receiver uses e.g. p13, p14 and 115200 baud. |
LukeMar | 0:8ed926c84e2f | 27 | _rx.set_blocking(false); // want receiver to not block |
LukeMar | 0:8ed926c84e2f | 28 | period_ms = 11; // start with 11ms period. |
LukeMar | 0:8ed926c84e2f | 29 | |
LukeMar | 0:8ed926c84e2f | 30 | // initialize public variables... |
LukeMar | 0:8ed926c84e2f | 31 | system = 0; // should be 0xa2 or 0xb2 for DSMX modes |
LukeMar | 0:8ed926c84e2f | 32 | fades = 0; |
LukeMar | 0:8ed926c84e2f | 33 | for (int i=0; i<SPEKTRUM_CHANNELS; i++){ |
LukeMar | 0:8ed926c84e2f | 34 | channel[i] = 0; |
LukeMar | 0:8ed926c84e2f | 35 | pulsewidth[i] = 0; |
LukeMar | 0:8ed926c84e2f | 36 | } |
LukeMar | 0:8ed926c84e2f | 37 | valid = false; |
LukeMar | 0:8ed926c84e2f | 38 | |
LukeMar | 0:8ed926c84e2f | 39 | // start the packet reading thread |
LukeMar | 0:8ed926c84e2f | 40 | _packet_thread.start(callback(this,&Spektrum::_packet_callback)); |
LukeMar | 0:8ed926c84e2f | 41 | } // Spektrum(tx, rx) constructor |
LukeMar | 0:8ed926c84e2f | 42 | |
LukeMar | 0:8ed926c84e2f | 43 | |
LukeMar | 0:8ed926c84e2f | 44 | |
LukeMar | 0:8ed926c84e2f | 45 | |
LukeMar | 0:8ed926c84e2f | 46 | /** Destructor for Spektrum object |
LukeMar | 0:8ed926c84e2f | 47 | */ |
LukeMar | 0:8ed926c84e2f | 48 | Spektrum::~Spektrum(){ |
LukeMar | 0:8ed926c84e2f | 49 | } // ~Spektrum() destructor |
LukeMar | 0:8ed926c84e2f | 50 | |
LukeMar | 0:8ed926c84e2f | 51 | |
LukeMar | 0:8ed926c84e2f | 52 | |
LukeMar | 0:8ed926c84e2f | 53 | /** Private callback for when a packet is received |
LukeMar | 0:8ed926c84e2f | 54 | */ |
LukeMar | 0:8ed926c84e2f | 55 | void Spektrum::_packet_callback(void){ |
LukeMar | 0:8ed926c84e2f | 56 | // local variables |
LukeMar | 0:8ed926c84e2f | 57 | int count; // used to get error code -11 or num of bytes _rx.read() |
LukeMar | 0:8ed926c84e2f | 58 | uint64_t now; // for getting 11 or 22 ms period via ThisThread::sleep_until() |
LukeMar | 0:8ed926c84e2f | 59 | unsigned int i; // counter for for loop |
LukeMar | 0:8ed926c84e2f | 60 | unsigned int data; // for assembling 2 bytes into uint16_t |
LukeMar | 0:8ed926c84e2f | 61 | unsigned int chanid; // for decoding channel ID with mask 0x7800 |
LukeMar | 0:8ed926c84e2f | 62 | unsigned int servopos; // for decoding servo value with mask 0x07ff |
LukeMar | 0:8ed926c84e2f | 63 | |
LukeMar | 0:8ed926c84e2f | 64 | // setup |
LukeMar | 0:8ed926c84e2f | 65 | //debug("Spektrum::_packet_thread started\r\n"); |
LukeMar | 0:8ed926c84e2f | 66 | |
LukeMar | 0:8ed926c84e2f | 67 | // loop |
LukeMar | 0:8ed926c84e2f | 68 | _rx.sync(); // flush buffer |
LukeMar | 0:8ed926c84e2f | 69 | while(1){ |
LukeMar | 0:8ed926c84e2f | 70 | now = rtos::Kernel::get_ms_count(); // for timing |
LukeMar | 0:8ed926c84e2f | 71 | count = _rx.read(_buf,SPEKTRUM_PACKET_SIZE); // try to read packet |
LukeMar | 0:8ed926c84e2f | 72 | |
LukeMar | 0:8ed926c84e2f | 73 | if (count == SPEKTRUM_PACKET_SIZE){ |
LukeMar | 0:8ed926c84e2f | 74 | // got a full sized packet |
LukeMar | 0:8ed926c84e2f | 75 | if (_buf[1] == SPEKTRUM_22MS_2048_DSMX){ |
LukeMar | 0:8ed926c84e2f | 76 | period_ms = 22; |
LukeMar | 0:8ed926c84e2f | 77 | valid = true; |
LukeMar | 0:8ed926c84e2f | 78 | } // got 22ms packets |
LukeMar | 0:8ed926c84e2f | 79 | else if (_buf[1] == SPEKTRUM_11MS_2048_DSMX){ |
LukeMar | 0:8ed926c84e2f | 80 | period_ms = 11; |
LukeMar | 0:8ed926c84e2f | 81 | valid = true; |
LukeMar | 0:8ed926c84e2f | 82 | } // got 11ms packets |
LukeMar | 0:8ed926c84e2f | 83 | else |
LukeMar | 0:8ed926c84e2f | 84 | // if system is not 0xa2 or 0xb2, treat as invalid |
LukeMar | 0:8ed926c84e2f | 85 | valid = false; |
LukeMar | 0:8ed926c84e2f | 86 | } // if count == 16 |
LukeMar | 0:8ed926c84e2f | 87 | else { |
LukeMar | 0:8ed926c84e2f | 88 | // count wasn't 16 so some kind of error |
LukeMar | 0:8ed926c84e2f | 89 | valid = false; |
LukeMar | 0:8ed926c84e2f | 90 | _rx.sync(); // not getting enough bytes, so sync() |
LukeMar | 0:8ed926c84e2f | 91 | } |
LukeMar | 0:8ed926c84e2f | 92 | |
LukeMar | 0:8ed926c84e2f | 93 | if (valid){ |
LukeMar | 0:8ed926c84e2f | 94 | // got a valid packet so parse it |
LukeMar | 0:8ed926c84e2f | 95 | fades = _buf[0]; |
LukeMar | 0:8ed926c84e2f | 96 | system = _buf[1]; |
LukeMar | 0:8ed926c84e2f | 97 | for (i=0; i<SPEKTRUM_SERVOS; i++){ |
LukeMar | 0:8ed926c84e2f | 98 | data = (_buf[2*i+2]<<8) | _buf[2*i+2+1]; |
LukeMar | 0:8ed926c84e2f | 99 | chanid = (data & SPEKTRUM_MASK_2048_CHANID) >> 11; |
LukeMar | 0:8ed926c84e2f | 100 | servopos = data & SPEKTRUM_MASK_2048_SXPOS; |
LukeMar | 0:8ed926c84e2f | 101 | channel[chanid] = servopos; |
LukeMar | 0:8ed926c84e2f | 102 | pulsewidth[chanid] = SPEKTRUM_COUNT2US(servopos); |
LukeMar | 0:8ed926c84e2f | 103 | } // for each servo in packet |
LukeMar | 0:8ed926c84e2f | 104 | } // if(valid) |
LukeMar | 0:8ed926c84e2f | 105 | |
LukeMar | 0:8ed926c84e2f | 106 | ThisThread::sleep_until(now+period_ms); // sleep to get right rate |
LukeMar | 0:8ed926c84e2f | 107 | } // while(1) |
LukeMar | 0:8ed926c84e2f | 108 | } // _packet_callback() |
LukeMar | 0:8ed926c84e2f | 109 | |
LukeMar | 0:8ed926c84e2f | 110 | |
LukeMar | 0:8ed926c84e2f | 111 | |
LukeMar | 0:8ed926c84e2f | 112 | |
LukeMar | 0:8ed926c84e2f | 113 | |
LukeMar | 0:8ed926c84e2f | 114 | |
LukeMar | 0:8ed926c84e2f | 115 | |
LukeMar | 0:8ed926c84e2f | 116 | |
LukeMar | 0:8ed926c84e2f | 117 | |
LukeMar | 0:8ed926c84e2f | 118 | /** BindPlug is used to bind a Spektrum Satellite receiver |
LukeMar | 0:8ed926c84e2f | 119 | @param tx orange wire, 3.3V supply pin, here used as a DigitalOut |
LukeMar | 0:8ed926c84e2f | 120 | @param rx gray wire, rx pin, here used as a DigitalOut |
LukeMar | 0:8ed926c84e2f | 121 | @param mode is mode, e.g. internal or external, DSM2 or DSMX, 11 or 22ms |
LukeMar | 0:8ed926c84e2f | 122 | The black wire should be connected to ground. |
LukeMar | 0:8ed926c84e2f | 123 | Default mode is internal, DSMX, 11 ms. Once created, this object will |
LukeMar | 0:8ed926c84e2f | 124 | send a number of falling pulses over the 3.3V supply pin to trigger |
LukeMar | 0:8ed926c84e2f | 125 | the satellite receiver to go into bind mode. |
LukeMar | 0:8ed926c84e2f | 126 | */ |
LukeMar | 0:8ed926c84e2f | 127 | BindPlug::BindPlug(PinName tx, PinName rx, int mode): _3Vpin(tx),_datapin(rx){ |
LukeMar | 0:8ed926c84e2f | 128 | int i; // counter |
LukeMar | 0:8ed926c84e2f | 129 | |
LukeMar | 0:8ed926c84e2f | 130 | // within 200 ms of applying power, supply a bunch of falling pulses |
LukeMar | 0:8ed926c84e2f | 131 | // according to table in Spektrum docs, most likely 9 pulses for |
LukeMar | 0:8ed926c84e2f | 132 | // internal mode, DSMX, 11 ms. |
LukeMar | 0:8ed926c84e2f | 133 | _3Vpin = 0; |
LukeMar | 0:8ed926c84e2f | 134 | _datapin = 0; |
LukeMar | 0:8ed926c84e2f | 135 | wait(1); |
LukeMar | 0:8ed926c84e2f | 136 | _3Vpin = 1; |
LukeMar | 0:8ed926c84e2f | 137 | _datapin = 1; |
LukeMar | 0:8ed926c84e2f | 138 | wait_ms(72); |
LukeMar | 0:8ed926c84e2f | 139 | debug("pulse "); |
LukeMar | 0:8ed926c84e2f | 140 | for(i=0; i<mode; i++){ |
LukeMar | 0:8ed926c84e2f | 141 | debug("%d ",i); |
LukeMar | 0:8ed926c84e2f | 142 | wait_us(116); |
LukeMar | 0:8ed926c84e2f | 143 | _datapin = 0; // this is the falling pulse |
LukeMar | 0:8ed926c84e2f | 144 | wait_us(116); |
LukeMar | 0:8ed926c84e2f | 145 | _datapin = 1; |
LukeMar | 0:8ed926c84e2f | 146 | } |
LukeMar | 0:8ed926c84e2f | 147 | debug("\r\n"); |
LukeMar | 0:8ed926c84e2f | 148 | } // BindPlug(bind, mode) constructor |
LukeMar | 0:8ed926c84e2f | 149 | |
LukeMar | 0:8ed926c84e2f | 150 | /** Destructor for BindPlug object |
LukeMar | 0:8ed926c84e2f | 151 | */ |
LukeMar | 0:8ed926c84e2f | 152 | BindPlug::~BindPlug(){ |
LukeMar | 0:8ed926c84e2f | 153 | } // ~BindPlug() destructor |
LukeMar | 0:8ed926c84e2f | 154 | |
LukeMar | 0:8ed926c84e2f | 155 | |
LukeMar | 0:8ed926c84e2f | 156 | |
LukeMar | 0:8ed926c84e2f | 157 | |
LukeMar | 0:8ed926c84e2f | 158 | |
LukeMar | 0:8ed926c84e2f | 159 | |
LukeMar | 0:8ed926c84e2f | 160 | /* LATER |
LukeMar | 0:8ed926c84e2f | 161 | SpektrumTestDevice::SpektrumTestDevice(PinName tx, PinName rx): _receiver(tx,rx){ |
LukeMar | 0:8ed926c84e2f | 162 | _receiver.baud(SPEKTRUM_BAUD); |
LukeMar | 0:8ed926c84e2f | 163 | } // SpektrumTestDevice(tx, rx) constructor |
LukeMar | 0:8ed926c84e2f | 164 | SpektrumTestDevice::~SpektrumTestDevice(){ |
LukeMar | 0:8ed926c84e2f | 165 | } // ~SpektrumTestDevice() destructor |
LukeMar | 0:8ed926c84e2f | 166 | */ |