Matthew Marino / Mbed 2 deprecated FINAL_SAILBOT_CODE

JAN 16 2019

Dependencies:   mbed BNO055 MBed_Adafruit-GPS-Library

Spektrum.cpp@1:4b4d5d18fc57, 2019-03-14 (annotated)

Committer:
LukeMar
Date:
Thu Mar 14 22:09:48 2019 +0000
Revision:
1:4b4d5d18fc57
Parent:
0:f4edd3407cc5 
3/14/19

Who changed what in which revision?

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