Suga koubou / FutabaSBUS

my sbus

Dependents:   Drone_air

Fork of FutabaSBUS by Uwe Gartmann

FutabaSBUS.cpp@2:07dbb77a6f1a, 2016-02-08 (annotated)

Committer:
okini3939
Date:
Mon Feb 08 08:47:36 2016 +0000
Revision:
2:07dbb77a6f1a
Parent:
1:e3c92fba87f2
Child:
3:50e10bf74374
fix serial;

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