diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c
index b2028e8efc..299ff4fce2 100755
--- a/src/main/rx/ibus.c
+++ b/src/main/rx/ibus.c
@@ -13,13 +13,19 @@
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see .
+ *
+ *
+ * Driver for IBUS (Flysky) receiver
+ * - initial implementation for MultiWii by Cesco/Plüschi
+ * - implementation for BaseFlight by Andreas (fiendie) Tacke
+ * - ported to CleanFlight by Konstantin (digitalentity) Sharlaimov
*/
#include
#include
#include
-#include "platform.h"
+#include
#include "build_config.h"
@@ -32,9 +38,7 @@
#include "rx/rx.h"
#include "rx/ibus.h"
-// Driver for IBUS (Flysky) receiver
-
-#define IBUS_MAX_CHANNEL 8
+#define IBUS_MAX_CHANNEL 10
#define IBUS_BUFFSIZE 32
#define IBUS_SYNCBYTE 0x20
@@ -95,7 +99,7 @@ static void ibusDataReceive(uint16_t c)
uint8_t ibusFrameStatus(void)
{
- uint8_t i;
+ uint8_t i, offset;
uint8_t frameStatus = SERIAL_RX_FRAME_PENDING;
uint16_t chksum, rxsum;
@@ -112,15 +116,9 @@ uint8_t ibusFrameStatus(void)
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];
-
+ for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
+ ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
+ }
frameStatus = SERIAL_RX_FRAME_COMPLETE;
}
diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h
index f206052d83..1106659fd5 100755
--- a/src/main/rx/ibus.h
+++ b/src/main/rx/ibus.h
@@ -18,3 +18,4 @@
#pragma once
uint8_t ibusFrameStatus(void);
+bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);