Suga koubou / FutabaSBUS

my sbus

Dependents:   Drone_air

Fork of FutabaSBUS by Uwe Gartmann

FutabaSBUS.cpp@1:e3c92fba87f2, 2012-02-09 (annotated)

Committer:
Digixx
Date:
Thu Feb 09 21:50:00 2012 +0000
Revision:
1:e3c92fba87f2
Parent:
0:6618cf21c95c
Child:
2:07dbb77a6f1a
1.0

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"
Digixx 0:6618cf21c95c 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) {
Digixx 1:e3c92fba87f2 40 // Set Baudrate
Digixx 0:6618cf21c95c 41 sbus_.baud(100000);
Digixx 1:e3c92fba87f2 42 // Set Datalenght & Frame
Digixx 0:6618cf21c95c 43 sbus_.format(8, Serial::Even, 2);
Digixx 1:e3c92fba87f2 44 // Attach interrupt routines
Digixx 0:6618cf21c95c 45 sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, MODSERIAL::RxIrq);
Digixx 1:e3c92fba87f2 46 // init ticker 500us
Digixx 0:6618cf21c95c 47 rxSBUS.attach_us(this, &FutabaSBUS::rx_ticker_500us,500);
Digixx 0:6618cf21c95c 48 rx_timeout=50;
Digixx 0:6618cf21c95c 49 tx_timeout=60;
Digixx 0:6618cf21c95c 50 }
Digixx 0:6618cf21c95c 51
Digixx 0:6618cf21c95c 52 int16_t FutabaSBUS::channel(uint8_t ch) {
Digixx 1:e3c92fba87f2 53 // Read channel data
Digixx 0:6618cf21c95c 54 if ((ch>0)&&(ch<=16)){
Digixx 0:6618cf21c95c 55 return channels[ch-1];
Digixx 0:6618cf21c95c 56 }else{
Digixx 0:6618cf21c95c 57 return 1023;
Digixx 0:6618cf21c95c 58 }
Digixx 0:6618cf21c95c 59 }
Digixx 0:6618cf21c95c 60
Digixx 0:6618cf21c95c 61 uint8_t FutabaSBUS::digichannel(uint8_t ch) {
Digixx 1:e3c92fba87f2 62 // Read digital channel data
Digixx 0:6618cf21c95c 63 if ((ch>0) && (ch<=2)) {
Digixx 0:6618cf21c95c 64 return channels[15+ch];
Digixx 0:6618cf21c95c 65 }else{
Digixx 0:6618cf21c95c 66 return 0;
Digixx 0:6618cf21c95c 67 }
Digixx 0:6618cf21c95c 68 }
Digixx 0:6618cf21c95c 69
Digixx 0:6618cf21c95c 70 void FutabaSBUS::servo(uint8_t ch, int16_t position) {
Digixx 1:e3c92fba87f2 71 // Set servo position
Digixx 0:6618cf21c95c 72 if ((ch>0)&&(ch<=16)) {
Digixx 0:6618cf21c95c 73 if (position>2048) {position=2048;}
Digixx 0:6618cf21c95c 74 servos[ch-1] = position;
Digixx 0:6618cf21c95c 75 }
Digixx 0:6618cf21c95c 76 }
Digixx 0:6618cf21c95c 77
Digixx 0:6618cf21c95c 78 void FutabaSBUS::digiservo(uint8_t ch, uint8_t position) {
Digixx 1:e3c92fba87f2 79 // Set digital servo position
Digixx 0:6618cf21c95c 80 if ((ch>0) && (ch<=2)) {
Digixx 0:6618cf21c95c 81 if (position>1) {position=1;}
Digixx 0:6618cf21c95c 82 servos[15+ch] = position;
Digixx 0:6618cf21c95c 83 }
Digixx 0:6618cf21c95c 84 }
Digixx 0:6618cf21c95c 85
Digixx 0:6618cf21c95c 86 uint8_t FutabaSBUS::failsafe(void) {return failsafe_status;}
Digixx 0:6618cf21c95c 87
Digixx 0:6618cf21c95c 88 void FutabaSBUS::passthrough(bool mode) {
Digixx 1:e3c92fba87f2 89 // Set passtrough mode, if true, received channel data is send to servos
Digixx 0:6618cf21c95c 90 sbus_passthrough = mode;
Digixx 0:6618cf21c95c 91 }
Digixx 0:6618cf21c95c 92
Digixx 0:6618cf21c95c 93 bool FutabaSBUS::passthrough(void) {
Digixx 1:e3c92fba87f2 94 // Return current passthrough mode
Digixx 0:6618cf21c95c 95 return sbus_passthrough;
Digixx 0:6618cf21c95c 96 }
Digixx 0:6618cf21c95c 97
Digixx 0:6618cf21c95c 98 /****************************************************************/
Digixx 0:6618cf21c95c 99 /****************************************************************/
Digixx 0:6618cf21c95c 100
Digixx 0:6618cf21c95c 101 void FutabaSBUS::SBUS_irq_rx(MODSERIAL_IRQ_INFO *q) {
Digixx 0:6618cf21c95c 102 rx_timeout=2;
Digixx 0:6618cf21c95c 103 tx_timeout=4;
Digixx 0:6618cf21c95c 104 }
Digixx 0:6618cf21c95c 105
Digixx 0:6618cf21c95c 106 void FutabaSBUS::update_channels(void) {
Digixx 1:e3c92fba87f2 107 // Read all received data and calculate channel data
Digixx 0:6618cf21c95c 108 uint8_t i;
Digixx 0:6618cf21c95c 109 uint8_t sbus_pointer = 0;
Digixx 0:6618cf21c95c 110 while (sbus_.readable()) {
Digixx 0:6618cf21c95c 111 uint8_t data = sbus_.getc(); // get data from serial rx buffer
Digixx 0:6618cf21c95c 112 switch (sbus_pointer) {
Digixx 0:6618cf21c95c 113 case 0: // Byte 1
Digixx 0:6618cf21c95c 114 if (data==0x0f) {
Digixx 0:6618cf21c95c 115 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 116 sbus_pointer++;
Digixx 0:6618cf21c95c 117 }
Digixx 0:6618cf21c95c 118 break;
Digixx 0:6618cf21c95c 119
Digixx 0:6618cf21c95c 120 case 24: // Byte 25 >> if last byte == 0x00 >> convert data
Digixx 0:6618cf21c95c 121 if (data==0x00) {
Digixx 0:6618cf21c95c 122 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 123 // clear channels[]
Digixx 0:6618cf21c95c 124 for (i=0; i<16; i++) {channels[i] = 0;}
Digixx 0:6618cf21c95c 125
Digixx 0:6618cf21c95c 126 // reset counters
Digixx 0:6618cf21c95c 127 uint8_t byte_in_sbus = 1;
Digixx 0:6618cf21c95c 128 uint8_t bit_in_sbus = 0;
Digixx 0:6618cf21c95c 129 uint8_t ch = 0;
Digixx 0:6618cf21c95c 130 uint8_t bit_in_channel = 0;
Digixx 0:6618cf21c95c 131
Digixx 0:6618cf21c95c 132 // process actual sbus data
Digixx 0:6618cf21c95c 133 for (i=0; i<176; i++) {
Digixx 0:6618cf21c95c 134 if (sbus_data[byte_in_sbus] & (1<<bit_in_sbus)) {
Digixx 0:6618cf21c95c 135 channels[ch] |= (1<<bit_in_channel);
Digixx 0:6618cf21c95c 136 }
Digixx 0:6618cf21c95c 137 bit_in_sbus++;
Digixx 0:6618cf21c95c 138 bit_in_channel++;
Digixx 0:6618cf21c95c 139
Digixx 0:6618cf21c95c 140 if (bit_in_sbus == 8) {
Digixx 0:6618cf21c95c 141 bit_in_sbus =0;
Digixx 0:6618cf21c95c 142 byte_in_sbus++;
Digixx 0:6618cf21c95c 143 }
Digixx 0:6618cf21c95c 144 if (bit_in_channel == 11) {
Digixx 0:6618cf21c95c 145 bit_in_channel =0;
Digixx 0:6618cf21c95c 146 ch++;
Digixx 0:6618cf21c95c 147 }
Digixx 0:6618cf21c95c 148 }
Digixx 0:6618cf21c95c 149 // DigiChannel 1
Digixx 0:6618cf21c95c 150 if (sbus_data[23] & (1<<0)) {
Digixx 0:6618cf21c95c 151 channels[16] = 1;
Digixx 0:6618cf21c95c 152 }else{
Digixx 0:6618cf21c95c 153 channels[16] = 0;
Digixx 0:6618cf21c95c 154 }
Digixx 0:6618cf21c95c 155 // DigiChannel 2
Digixx 0:6618cf21c95c 156 if (sbus_data[23] & (1<<1)) {
Digixx 0:6618cf21c95c 157 channels[17] = 1;
Digixx 0:6618cf21c95c 158 }else{
Digixx 0:6618cf21c95c 159 channels[17] = 0;
Digixx 0:6618cf21c95c 160 }
Digixx 0:6618cf21c95c 161 // Failsafe
Digixx 0:6618cf21c95c 162 failsafe_status = SBUS_SIGNAL_OK;
Digixx 0:6618cf21c95c 163 if (sbus_data[23] & (1<<2)) {
Digixx 0:6618cf21c95c 164 failsafe_status = SBUS_SIGNAL_LOST;
Digixx 0:6618cf21c95c 165 }
Digixx 0:6618cf21c95c 166 if (sbus_data[23] & (1<<3)) {
Digixx 0:6618cf21c95c 167 failsafe_status = SBUS_SIGNAL_FAILSAFE;
Digixx 0:6618cf21c95c 168 }
Digixx 0:6618cf21c95c 169 }
Digixx 0:6618cf21c95c 170 break;
Digixx 0:6618cf21c95c 171
Digixx 0:6618cf21c95c 172 default: // collect Channel data (11bit) / Failsafe information
Digixx 0:6618cf21c95c 173 sbus_data[sbus_pointer] = data;
Digixx 0:6618cf21c95c 174 sbus_pointer++;
Digixx 0:6618cf21c95c 175 }
Digixx 0:6618cf21c95c 176 }
Digixx 0:6618cf21c95c 177 }
Digixx 0:6618cf21c95c 178
Digixx 1:e3c92fba87f2 179 void FutabaSBUS::update_servos(void) {
Digixx 1:e3c92fba87f2 180 // Send data to servos
Digixx 1:e3c92fba87f2 181 // Passtrough mode = false >> send own servo data
Digixx 1:e3c92fba87f2 182 // Passtrough mode = true >> send received channel data
Digixx 1:e3c92fba87f2 183 uint8_t i;
Digixx 1:e3c92fba87f2 184 if (!sbus_passthrough) {
Digixx 1:e3c92fba87f2 185 // clear received channel data
Digixx 1:e3c92fba87f2 186 for (i=1; i<24; i++) {
Digixx 1:e3c92fba87f2 187 sbus_data[i] = 0;
Digixx 1:e3c92fba87f2 188 }
Digixx 1:e3c92fba87f2 189
Digixx 1:e3c92fba87f2 190 // reset counters
Digixx 1:e3c92fba87f2 191 uint8_t ch = 0;
Digixx 1:e3c92fba87f2 192 uint8_t bit_in_servo = 0;
Digixx 1:e3c92fba87f2 193 uint8_t byte_in_sbus = 1;
Digixx 1:e3c92fba87f2 194 uint8_t bit_in_sbus = 0;
Digixx 1:e3c92fba87f2 195
Digixx 1:e3c92fba87f2 196 // store servo data
Digixx 1:e3c92fba87f2 197 for (i=0; i<176; i++) {
Digixx 1:e3c92fba87f2 198 if (servos[ch] & (1<<bit_in_servo)) {
Digixx 1:e3c92fba87f2 199 sbus_data[byte_in_sbus] |= (1<<bit_in_sbus);
Digixx 1:e3c92fba87f2 200 }
Digixx 1:e3c92fba87f2 201 bit_in_sbus++;
Digixx 1:e3c92fba87f2 202 bit_in_servo++;
Digixx 1:e3c92fba87f2 203
Digixx 1:e3c92fba87f2 204 if (bit_in_sbus == 8) {
Digixx 1:e3c92fba87f2 205 bit_in_sbus =0;
Digixx 1:e3c92fba87f2 206 byte_in_sbus++;
Digixx 1:e3c92fba87f2 207 }
Digixx 1:e3c92fba87f2 208 if (bit_in_servo == 11) {
Digixx 1:e3c92fba87f2 209 bit_in_servo =0;
Digixx 1:e3c92fba87f2 210 ch++;
Digixx 1:e3c92fba87f2 211 }
Digixx 1:e3c92fba87f2 212 }
Digixx 1:e3c92fba87f2 213
Digixx 1:e3c92fba87f2 214 // DigiChannel 1
Digixx 1:e3c92fba87f2 215 if (channels[16] == 1) {
Digixx 1:e3c92fba87f2 216 sbus_data[23] |= (1<<0);
Digixx 1:e3c92fba87f2 217 }
Digixx 1:e3c92fba87f2 218 // DigiChannel 2
Digixx 1:e3c92fba87f2 219 if (channels[17] == 1) {
Digixx 1:e3c92fba87f2 220 sbus_data[23] |= (1<<1);
Digixx 1:e3c92fba87f2 221 }
Digixx 1:e3c92fba87f2 222
Digixx 1:e3c92fba87f2 223 // Failsafe
Digixx 1:e3c92fba87f2 224 if (failsafe_status == SBUS_SIGNAL_LOST) {
Digixx 1:e3c92fba87f2 225 sbus_data[23] |= (1<<2);
Digixx 1:e3c92fba87f2 226 }
Digixx 1:e3c92fba87f2 227
Digixx 1:e3c92fba87f2 228 if (failsafe_status == SBUS_SIGNAL_FAILSAFE) {
Digixx 1:e3c92fba87f2 229 sbus_data[23] |= (1<<2);
Digixx 1:e3c92fba87f2 230 sbus_data[23] |= (1<<3);
Digixx 1:e3c92fba87f2 231 }
Digixx 1:e3c92fba87f2 232 }
Digixx 1:e3c92fba87f2 233 // send data out
Digixx 1:e3c92fba87f2 234 for (i=0;i<25;i++) {
Digixx 1:e3c92fba87f2 235 sbus_.putc(sbus_data[i]);
Digixx 1:e3c92fba87f2 236 }
Digixx 1:e3c92fba87f2 237 }
Digixx 0:6618cf21c95c 238
Digixx 0:6618cf21c95c 239 void FutabaSBUS::rx_ticker_500us(void) {
Digixx 0:6618cf21c95c 240 // RX
Digixx 0:6618cf21c95c 241 switch (rx_timeout) {
Digixx 0:6618cf21c95c 242 case 0:
Digixx 0:6618cf21c95c 243 break;
Digixx 0:6618cf21c95c 244 case 1:
Digixx 0:6618cf21c95c 245 if (sbus_.readable()) {update_channels();}
Digixx 0:6618cf21c95c 246 default:
Digixx 0:6618cf21c95c 247 rx_timeout--;
Digixx 0:6618cf21c95c 248 }
Digixx 0:6618cf21c95c 249 // TX
Digixx 0:6618cf21c95c 250 switch (tx_timeout) {
Digixx 0:6618cf21c95c 251 case 0:
Digixx 0:6618cf21c95c 252 update_servos();
Digixx 0:6618cf21c95c 253 tx_timeout = 28;
Digixx 0:6618cf21c95c 254 default:
Digixx 0:6618cf21c95c 255 tx_timeout--;
Digixx 0:6618cf21c95c 256 }
Digixx 0:6618cf21c95c 257 }