Suga koubou / FutabaSBUS

my sbus

Dependents:   Drone_air

Fork of FutabaSBUS by Uwe Gartmann

FutabaSBUS.cpp@3:50e10bf74374, 2016-05-19 (annotated)

Committer:
okini3939
Date:
Thu May 19 08:56:30 2016 +0000
Revision:
3:50e10bf74374
Parent:
2:07dbb77a6f1a 
added start();

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Digixx 0:6618cf21c95c 1 /* mbed R/C Futaba SBUS Library
Digixx 0:6618cf21c95c 2 * Copyright (c) 2011-2012 digixx
Digixx 0:6618cf21c95c 3 *
Digixx 0:6618cf21c95c 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
Digixx 0:6618cf21c95c 5 * of this software and associated documentation files (the "Software"), to deal
Digixx 0:6618cf21c95c 6 * in the Software without restriction, including without limitation the rights
Digixx 0:6618cf21c95c 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Digixx 0:6618cf21c95c 8 * copies of the Software, and to permit persons to whom the Software is
Digixx 0:6618cf21c95c 9 * furnished to do so, subject to the following conditions:
Digixx 0:6618cf21c95c 10 *
Digixx 0:6618cf21c95c 11 * The above copyright notice and this permission notice shall be included in
Digixx 0:6618cf21c95c 12 * all copies or substantial portions of the Software.
Digixx 0:6618cf21c95c 13 *
Digixx 0:6618cf21c95c 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Digixx 0:6618cf21c95c 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Digixx 0:6618cf21c95c 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Digixx 0:6618cf21c95c 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Digixx 0:6618cf21c95c 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Digixx 0:6618cf21c95c 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Digixx 0:6618cf21c95c 20 * THE SOFTWARE.
Digixx 0:6618cf21c95c 21 */
Digixx 0:6618cf21c95c 22
Digixx 0:6618cf21c95c 23 #include "FutabaSBUS.h"
okini3939 2:07dbb77a6f1a 24 //#include "MODSERIAL.h"
Digixx 0:6618cf21c95c 25 #include "mbed.h"
Digixx 0:6618cf21c95c 26
Digixx 0:6618cf21c95c 27 //debug only
Digixx 0:6618cf21c95c 28 DigitalOut tst1(p8);
Digixx 0:6618cf21c95c 29 DigitalOut tst2(p9);
Digixx 0:6618cf21c95c 30 DigitalOut tst3(p10);
Digixx 0:6618cf21c95c 31
Digixx 0:6618cf21c95c 32 uint8_t sbus_data[25] = {0x0f,0x01,0x04,0x20,0x00,0xff,0x07,0x40,0x00,0x02,0x10,0x80,0x2c,0x64,0x21,0x0b,0x59,0x08,0x40,0x00,0x02,0x10,0x80,0x00,0x00};
Digixx 1:e3c92fba87f2 33 int16_t channels[18] = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
Digixx 1:e3c92fba87f2 34 int16_t servos[18] = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
Digixx 0:6618cf21c95c 35 uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE;
Digixx 0:6618cf21c95c 36 bool sbus_passthrough = true;
Digixx 0:6618cf21c95c 37
Digixx 0:6618cf21c95c 38
Digixx 0:6618cf21c95c 39 FutabaSBUS::FutabaSBUS(PinName tx, PinName rx) : sbus_(tx, rx) {
okini3939 3:50e10bf74374 40 rx_timeout=50;
okini3939 3:50e10bf74374 41 tx_timeout=60;
okini3939 3:50e10bf74374 42 }
okini3939 3:50e10bf74374 43
okini3939 3:50e10bf74374 44 void FutabaSBUS::start() {
Digixx 1:e3c92fba87f2 45 // Set Baudrate
Digixx 0:6618cf21c95c 46 sbus_.baud(100000);
Digixx 1:e3c92fba87f2 47 // Set Datalenght & Frame
Digixx 0:6618cf21c95c 48 sbus_.format(8, Serial::Even, 2);
Digixx 1:e3c92fba87f2 49 // Attach interrupt routines
okini3939 2:07dbb77a6f1a 50 // sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, MODSERIAL::RxIrq);
okini3939 2:07dbb77a6f1a 51 sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, RawSerial::RxIrq);
Digixx 1:e3c92fba87f2 52 // init ticker 500us
Digixx 0:6618cf21c95c 53 rxSBUS.attach_us(this, &FutabaSBUS::rx_ticker_500us,500);
Digixx 0:6618cf21c95c 54 }
Digixx 0:6618cf21c95c 55
Digixx 0:6618cf21c95c 56 int16_t FutabaSBUS::channel(uint8_t ch) {
Digixx 1:e3c92fba87f2 57 // Read channel data
Digixx 0:6618cf21c95c 58 if ((ch>0)&&(ch<=16)){
Digixx 0:6618cf21c95c 59 return channels[ch-1];
Digixx 0:6618cf21c95c 60 }else{
Digixx 0:6618cf21c95c 61 return 1023;
Digixx 0:6618cf21c95c 62 }
Digixx 0:6618cf21c95c 63 }
Digixx 0:6618cf21c95c 64
Digixx 0:6618cf21c95c 65 uint8_t FutabaSBUS::digichannel(uint8_t ch) {
Digixx 1:e3c92fba87f2 66 // Read digital channel data
Digixx 0:6618cf21c95c 67 if ((ch>0) && (ch<=2)) {
Digixx 0:6618cf21c95c 68 return channels[15+ch];
Digixx 0:6618cf21c95c 69 }else{
Digixx 0:6618cf21c95c 70 return 0;
Digixx 0:6618cf21c95c 71 }
Digixx 0:6618cf21c95c 72 }
Digixx 0:6618cf21c95c 73
Digixx 0:6618cf21c95c 74 void FutabaSBUS::servo(uint8_t ch, int16_t position) {
Digixx 1:e3c92fba87f2 75 // Set servo position
Digixx 0:6618cf21c95c 76 if ((ch>0)&&(ch<=16)) {
Digixx 0:6618cf21c95c 77 if (position>2048) {position=2048;}
Digixx 0:6618cf21c95c 78 servos[ch-1] = position;
Digixx 0:6618cf21c95c 79 }
Digixx 0:6618cf21c95c 80 }
Digixx 0:6618cf21c95c 81
Digixx 0:6618cf21c95c 82 void FutabaSBUS::digiservo(uint8_t ch, uint8_t position) {
Digixx 1:e3c92fba87f2 83 // Set digital servo position
Digixx 0:6618cf21c95c 84 if ((ch>0) && (ch<=2)) {
Digixx 0:6618cf21c95c 85 if (position>1) {position=1;}
Digixx 0:6618cf21c95c 86 servos[15+ch] = position;
Digixx 0:6618cf21c95c 87 }
Digixx 0:6618cf21c95c 88 }
Digixx 0:6618cf21c95c 89
Digixx 0:6618cf21c95c 90 uint8_t FutabaSBUS::failsafe(void) {return failsafe_status;}
Digixx 0:6618cf21c95c 91
okini3939 2:07dbb77a6f1a 92 void FutabaSBUS::failsafe(uint8_t status) {
okini3939 2:07dbb77a6f1a 93 failsafe_status = status;
okini3939 2:07dbb77a6f1a 94 }
okini3939 2:07dbb77a6f1a 95
Digixx 0:6618cf21c95c 96 void FutabaSBUS::passthrough(bool mode) {
Digixx 1:e3c92fba87f2 97 // Set passtrough mode, if true, received channel data is send to servos
Digixx 0:6618cf21c95c 98 sbus_passthrough = mode;
Digixx 0:6618cf21c95c 99 }
Digixx 0:6618cf21c95c 100
Digixx 0:6618cf21c95c 101 bool FutabaSBUS::passthrough(void) {
Digixx 1:e3c92fba87f2 102 // Return current passthrough mode
Digixx 0:6618cf21c95c 103 return sbus_passthrough;
Digixx 0:6618cf21c95c 104 }
Digixx 0:6618cf21c95c 105
Digixx 0:6618cf21c95c 106 /****************************************************************/
Digixx 0:6618cf21c95c 107 /****************************************************************/
Digixx 0:6618cf21c95c 108
okini3939 2:07dbb77a6f1a 109 //void FutabaSBUS::SBUS_irq_rx(MODSERIAL_IRQ_INFO *q) {
okini3939 2:07dbb77a6f1a 110 void FutabaSBUS::SBUS_irq_rx(void) {
Digixx 0:6618cf21c95c 111 rx_timeout=2;
Digixx 0:6618cf21c95c 112 tx_timeout=4;
Digixx 0:6618cf21c95c 113 }
Digixx 0:6618cf21c95c 114
Digixx 0:6618cf21c95c 115 void FutabaSBUS::update_channels(void) {
Digixx 1:e3c92fba87f2 116 // Read all received data and calculate channel data
Digixx 0:6618cf21c95c 117 uint8_t i;
Digixx 0:6618cf21c95c 118 uint8_t sbus_pointer = 0;
Digixx 0:6618cf21c95c 119 while (sbus_.readable()) {
Digixx 0:6618cf21c95c 120 uint8_t data = sbus_.getc(); // get data from serial rx buffer
Digixx 0:6618cf21c95c 121 switch (sbus_pointer) {
Digixx 0:6618cf21c95c 122 case 0: // Byte 1
Digixx 0:6618cf21c95c 123 if (data==0x0f) {
Digixx 0:6618cf21c95c 124 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 125 sbus_pointer++;
Digixx 0:6618cf21c95c 126 }
Digixx 0:6618cf21c95c 127 break;
Digixx 0:6618cf21c95c 128
Digixx 0:6618cf21c95c 129 case 24: // Byte 25 >> if last byte == 0x00 >> convert data
Digixx 0:6618cf21c95c 130 if (data==0x00) {
Digixx 0:6618cf21c95c 131 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 132 // clear channels[]
Digixx 0:6618cf21c95c 133 for (i=0; i<16; i++) {channels[i] = 0;}
Digixx 0:6618cf21c95c 134
Digixx 0:6618cf21c95c 135 // reset counters
Digixx 0:6618cf21c95c 136 uint8_t byte_in_sbus = 1;
Digixx 0:6618cf21c95c 137 uint8_t bit_in_sbus = 0;
Digixx 0:6618cf21c95c 138 uint8_t ch = 0;
Digixx 0:6618cf21c95c 139 uint8_t bit_in_channel = 0;
Digixx 0:6618cf21c95c 140
Digixx 0:6618cf21c95c 141 // process actual sbus data
Digixx 0:6618cf21c95c 142 for (i=0; i<176; i++) {
Digixx 0:6618cf21c95c 143 if (sbus_data[byte_in_sbus] & (1<<bit_in_sbus)) {
Digixx 0:6618cf21c95c 144 channels[ch] |= (1<<bit_in_channel);
Digixx 0:6618cf21c95c 145 }
Digixx 0:6618cf21c95c 146 bit_in_sbus++;
Digixx 0:6618cf21c95c 147 bit_in_channel++;
Digixx 0:6618cf21c95c 148
Digixx 0:6618cf21c95c 149 if (bit_in_sbus == 8) {
Digixx 0:6618cf21c95c 150 bit_in_sbus =0;
Digixx 0:6618cf21c95c 151 byte_in_sbus++;
Digixx 0:6618cf21c95c 152 }
Digixx 0:6618cf21c95c 153 if (bit_in_channel == 11) {
Digixx 0:6618cf21c95c 154 bit_in_channel =0;
Digixx 0:6618cf21c95c 155 ch++;
Digixx 0:6618cf21c95c 156 }
Digixx 0:6618cf21c95c 157 }
Digixx 0:6618cf21c95c 158 // DigiChannel 1
Digixx 0:6618cf21c95c 159 if (sbus_data[23] & (1<<0)) {
Digixx 0:6618cf21c95c 160 channels[16] = 1;
Digixx 0:6618cf21c95c 161 }else{
Digixx 0:6618cf21c95c 162 channels[16] = 0;
Digixx 0:6618cf21c95c 163 }
Digixx 0:6618cf21c95c 164 // DigiChannel 2
Digixx 0:6618cf21c95c 165 if (sbus_data[23] & (1<<1)) {
Digixx 0:6618cf21c95c 166 channels[17] = 1;
Digixx 0:6618cf21c95c 167 }else{
Digixx 0:6618cf21c95c 168 channels[17] = 0;
Digixx 0:6618cf21c95c 169 }
Digixx 0:6618cf21c95c 170 // Failsafe
Digixx 0:6618cf21c95c 171 failsafe_status = SBUS_SIGNAL_OK;
Digixx 0:6618cf21c95c 172 if (sbus_data[23] & (1<<2)) {
Digixx 0:6618cf21c95c 173 failsafe_status = SBUS_SIGNAL_LOST;
Digixx 0:6618cf21c95c 174 }
Digixx 0:6618cf21c95c 175 if (sbus_data[23] & (1<<3)) {
Digixx 0:6618cf21c95c 176 failsafe_status = SBUS_SIGNAL_FAILSAFE;
Digixx 0:6618cf21c95c 177 }
Digixx 0:6618cf21c95c 178 }
Digixx 0:6618cf21c95c 179 break;
Digixx 0:6618cf21c95c 180
Digixx 0:6618cf21c95c 181 default: // collect Channel data (11bit) / Failsafe information
Digixx 0:6618cf21c95c 182 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 183 sbus_pointer++;
Digixx 0:6618cf21c95c 184 }
Digixx 0:6618cf21c95c 185 }
Digixx 0:6618cf21c95c 186 }
Digixx 0:6618cf21c95c 187
Digixx 1:e3c92fba87f2 188 void FutabaSBUS::update_servos(void) {
Digixx 1:e3c92fba87f2 189 // Send data to servos
Digixx 1:e3c92fba87f2 190 // Passtrough mode = false >> send own servo data
Digixx 1:e3c92fba87f2 191 // Passtrough mode = true >> send received channel data
Digixx 1:e3c92fba87f2 192 uint8_t i;
Digixx 1:e3c92fba87f2 193 if (!sbus_passthrough) {
Digixx 1:e3c92fba87f2 194 // clear received channel data
Digixx 1:e3c92fba87f2 195 for (i=1; i<24; i++) {
Digixx 1:e3c92fba87f2 196 sbus_data[i] = 0;
Digixx 1:e3c92fba87f2 197 }
Digixx 1:e3c92fba87f2 198
Digixx 1:e3c92fba87f2 199 // reset counters
Digixx 1:e3c92fba87f2 200 uint8_t ch = 0;
Digixx 1:e3c92fba87f2 201 uint8_t bit_in_servo = 0;
Digixx 1:e3c92fba87f2 202 uint8_t byte_in_sbus = 1;
Digixx 1:e3c92fba87f2 203 uint8_t bit_in_sbus = 0;
Digixx 1:e3c92fba87f2 204
Digixx 1:e3c92fba87f2 205 // store servo data
Digixx 1:e3c92fba87f2 206 for (i=0; i<176; i++) {
Digixx 1:e3c92fba87f2 207 if (servos[ch] & (1<<bit_in_servo)) {
Digixx 1:e3c92fba87f2 208 sbus_data[byte_in_sbus] |= (1<<bit_in_sbus);
Digixx 1:e3c92fba87f2 209 }
Digixx 1:e3c92fba87f2 210 bit_in_sbus++;
Digixx 1:e3c92fba87f2 211 bit_in_servo++;
Digixx 1:e3c92fba87f2 212
Digixx 1:e3c92fba87f2 213 if (bit_in_sbus == 8) {
Digixx 1:e3c92fba87f2 214 bit_in_sbus =0;
Digixx 1:e3c92fba87f2 215 byte_in_sbus++;
Digixx 1:e3c92fba87f2 216 }
Digixx 1:e3c92fba87f2 217 if (bit_in_servo == 11) {
Digixx 1:e3c92fba87f2 218 bit_in_servo =0;
Digixx 1:e3c92fba87f2 219 ch++;
Digixx 1:e3c92fba87f2 220 }
Digixx 1:e3c92fba87f2 221 }
Digixx 1:e3c92fba87f2 222
Digixx 1:e3c92fba87f2 223 // DigiChannel 1
Digixx 1:e3c92fba87f2 224 if (channels[16] == 1) {
Digixx 1:e3c92fba87f2 225 sbus_data[23] |= (1<<0);
Digixx 1:e3c92fba87f2 226 }
Digixx 1:e3c92fba87f2 227 // DigiChannel 2
Digixx 1:e3c92fba87f2 228 if (channels[17] == 1) {
Digixx 1:e3c92fba87f2 229 sbus_data[23] |= (1<<1);
Digixx 1:e3c92fba87f2 230 }
Digixx 1:e3c92fba87f2 231
Digixx 1:e3c92fba87f2 232 // Failsafe
Digixx 1:e3c92fba87f2 233 if (failsafe_status == SBUS_SIGNAL_LOST) {
Digixx 1:e3c92fba87f2 234 sbus_data[23] |= (1<<2);
Digixx 1:e3c92fba87f2 235 }
Digixx 1:e3c92fba87f2 236
Digixx 1:e3c92fba87f2 237 if (failsafe_status == SBUS_SIGNAL_FAILSAFE) {
Digixx 1:e3c92fba87f2 238 sbus_data[23] |= (1<<2);
Digixx 1:e3c92fba87f2 239 sbus_data[23] |= (1<<3);
Digixx 1:e3c92fba87f2 240 }
Digixx 1:e3c92fba87f2 241 }
Digixx 1:e3c92fba87f2 242 // send data out
Digixx 1:e3c92fba87f2 243 for (i=0;i<25;i++) {
Digixx 1:e3c92fba87f2 244 sbus_.putc(sbus_data[i]);
Digixx 1:e3c92fba87f2 245 }
Digixx 1:e3c92fba87f2 246 }
Digixx 0:6618cf21c95c 247
Digixx 0:6618cf21c95c 248 void FutabaSBUS::rx_ticker_500us(void) {
okini3939 2:07dbb77a6f1a 249 /* // RX
Digixx 0:6618cf21c95c 250 switch (rx_timeout) {
Digixx 0:6618cf21c95c 251 case 0:
Digixx 0:6618cf21c95c 252 break;
Digixx 0:6618cf21c95c 253 case 1:
Digixx 0:6618cf21c95c 254 if (sbus_.readable()) {update_channels();}
Digixx 0:6618cf21c95c 255 default:
Digixx 0:6618cf21c95c 256 rx_timeout--;
Digixx 0:6618cf21c95c 257 }
okini3939 2:07dbb77a6f1a 258 */
Digixx 0:6618cf21c95c 259 // TX
Digixx 0:6618cf21c95c 260 switch (tx_timeout) {
Digixx 0:6618cf21c95c 261 case 0:
Digixx 0:6618cf21c95c 262 update_servos();
Digixx 0:6618cf21c95c 263 tx_timeout = 28;
Digixx 0:6618cf21c95c 264 default:
Digixx 0:6618cf21c95c 265 tx_timeout--;
Digixx 0:6618cf21c95c 266 }
Digixx 0:6618cf21c95c 267 }