SBUS-Library_16channel - Futaba S-BUS Library. Let you control 16 servos a…

Users » Digixx » Code » SBUS-Library_16channel

Uwe Gartmann / Mbed 2 deprecated SBUS-Library_16channel

Futaba S-BUS Library. Let you control 16 servos and 2 digital channels

FutabaSBUS/FutabaSBUS.cpp@0:83e415034198, 2012-03-07 (annotated)

Committer:: Digixx
Date:: Wed Mar 07 18:18:43 2012 +0000
Revision:: 0:83e415034198

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	07 Mar 2012
Imports:	359
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	19

FutabaSBUS/FutabaSBUS.cpp@0:83e415034198, 2012-03-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning