Suga koubou / FutabaSBUS

my sbus

Dependents:   Drone_air

Fork of FutabaSBUS by Uwe Gartmann

FutabaSBUS.cpp@0:6618cf21c95c, 2011-12-14 (annotated)

Committer:
Digixx
Date:
Wed Dec 14 20:46:10 2011 +0000
Revision:
0:6618cf21c95c
Child:
1:e3c92fba87f2
beta version

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