From f269f653f581afe39ff81b802db6febfb3e4d051 Mon Sep 17 00:00:00 2001 From: digitalentity Date: Wed, 3 Jun 2015 21:22:56 +1000 Subject: [PATCH] Implemented support for FlySky i-Bus serial receiver. --- Makefile | 1 + src/main/rx/ibus.c | 134 +++++++++++++++++++++++++++++++++++++++++++++ src/main/rx/ibus.h | 20 +++++++ src/main/rx/rx.c | 7 +++ src/main/rx/rx.h | 3 +- 5 files changed, 164 insertions(+), 1 deletion(-) create mode 100755 src/main/rx/ibus.c create mode 100755 src/main/rx/ibus.h diff --git a/Makefile b/Makefile index 0638ea1c4..bb4fa996f 100755 --- a/Makefile +++ b/Makefile @@ -298,6 +298,7 @@ COMMON_SRC = build_config.c \ rx/sumh.c \ rx/spektrum.c \ rx/xbus.c \ + rx/ibus.c \ sensors/acceleration.c \ sensors/battery.c \ sensors/boardalignment.c \ diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c new file mode 100755 index 000000000..b2028e8ef --- /dev/null +++ b/src/main/rx/ibus.c @@ -0,0 +1,134 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build_config.h" + +#include "drivers/system.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/ibus.h" + +// Driver for IBUS (Flysky) receiver + +#define IBUS_MAX_CHANNEL 8 +#define IBUS_BUFFSIZE 32 +#define IBUS_SYNCBYTE 0x20 + +#define IBUS_BAUDRATE 115200 + +static bool ibusFrameDone = false; +static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; + +static void ibusDataReceive(uint16_t c); +static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); + +bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +{ + UNUSED(rxConfig); + + if (callback) + *callback = ibusReadRawRC; + + rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL; + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return false; + } + + serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); + + return ibusPort != NULL; +} + +static uint8_t ibus[IBUS_BUFFSIZE] = { 0, }; + +// Receive ISR callback +static void ibusDataReceive(uint16_t c) +{ + uint32_t ibusTime; + static uint32_t ibusTimeLast; + static uint8_t ibusFramePosition; + + ibusTime = micros(); + + if ((ibusTime - ibusTimeLast) > 3000) + ibusFramePosition = 0; + + ibusTimeLast = ibusTime; + + if (ibusFramePosition == 0 && c != IBUS_SYNCBYTE) + return; + + ibus[ibusFramePosition] = (uint8_t)c; + + if (ibusFramePosition == IBUS_BUFFSIZE - 1) { + ibusFrameDone = true; + } else { + ibusFramePosition++; + } +} + +uint8_t ibusFrameStatus(void) +{ + uint8_t i; + uint8_t frameStatus = SERIAL_RX_FRAME_PENDING; + uint16_t chksum, rxsum; + + if (!ibusFrameDone) { + return frameStatus; + } + + ibusFrameDone = false; + + chksum = 0xFFFF; + for (i = 0; i < 30; i++) + chksum -= ibus[i]; + + rxsum = ibus[30] + (ibus[31] << 8); + + if (chksum == rxsum) { + ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2]; + ibusChannelData[1] = (ibus[ 5] << 8) + ibus[ 4]; + ibusChannelData[2] = (ibus[ 7] << 8) + ibus[ 6]; + ibusChannelData[3] = (ibus[ 9] << 8) + ibus[ 8]; + ibusChannelData[4] = (ibus[11] << 8) + ibus[10]; + ibusChannelData[5] = (ibus[13] << 8) + ibus[12]; + ibusChannelData[6] = (ibus[15] << 8) + ibus[14]; + ibusChannelData[7] = (ibus[17] << 8) + ibus[16]; + + frameStatus = SERIAL_RX_FRAME_COMPLETE; + } + + return frameStatus; +} + +static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +{ + UNUSED(rxRuntimeConfig); + return ibusChannelData[chan]; +} diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h new file mode 100755 index 000000000..f206052d8 --- /dev/null +++ b/src/main/rx/ibus.h @@ -0,0 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +uint8_t ibusFrameStatus(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 382005af0..e77b2fdc9 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -49,6 +49,7 @@ #include "rx/sumh.h" #include "rx/msp.h" #include "rx/xbus.h" +#include "rx/ibus.h" #include "rx/rx.h" @@ -61,6 +62,7 @@ bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, r bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); @@ -234,6 +236,9 @@ void serialRxInit(rxConfig_t *rxConfig) rxRefreshRate = 11000; enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; + case SERIALRX_IBUS: + enabled = ibusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + break; } if (!enabled) { @@ -266,6 +271,8 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig) case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: return xBusFrameStatus(); + case SERIALRX_IBUS: + return ibusFrameStatus(); } return SERIAL_RX_FRAME_PENDING; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 19ae78eae..6ac43194c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -51,7 +51,8 @@ typedef enum { SERIALRX_SUMH = 4, SERIALRX_XBUS_MODE_B = 5, SERIALRX_XBUS_MODE_B_RJ01 = 6, - SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_RJ01 + SERIALRX_IBUS = 7, + SERIALRX_PROVIDER_MAX = SERIALRX_IBUS } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)