Matthew Marino / Mbed 2 deprecated sailbot_RUDDER

Rudder program

Dependencies:   mbed

Spektrum.cpp@1:9e727d54e80c, 2018-11-28 (annotated)

Committer:
LukeMar
Date:
Wed Nov 28 15:17:04 2018 +0000
Revision:
1:9e727d54e80c
Parent:
0:8ed926c84e2f 
RUdder version of sailbot RC code

Who changed what in which revision?

UserRevisionLine numberNew 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 */