diff --git a/src/main/drivers/rx_a7105.c b/src/main/drivers/rx_a7105.c
index ecef155c43..cb23008625 100644
--- a/src/main/drivers/rx_a7105.c
+++ b/src/main/drivers/rx_a7105.c
@@ -1,19 +1,19 @@
/*
-* 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 .
-*/
+ * 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
@@ -21,7 +21,7 @@
#include
-#ifdef USE_RX_A7105
+#ifdef USE_RX_FLYSKY
#include "drivers/rx_a7105.h"
#include "drivers/bus_spi.h"
@@ -32,7 +32,6 @@
#include "drivers/exti.h"
#include "drivers/time.h"
-
#ifdef RX_PA_TXEN_PIN
static IO_t txEnIO = IO_NONE;
#endif
@@ -42,22 +41,19 @@ static extiCallbackRec_t a7105extiCallbackRec;
static volatile uint32_t timeEvent = 0;
static volatile bool occurEvent = false;
-
void a7105extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
- if (IORead (rxIntIO) != 0)
- {
+
+ if (IORead (rxIntIO) != 0) {
timeEvent = micros();
occurEvent = true;
}
}
-
-void A7105Init (uint32_t id)
-{
+void A7105Init (uint32_t id) {
spiDeviceByInstance(RX_SPI_INSTANCE);
- rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); /* config receiver IRQ pin */
+ rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); /* config receiver IRQ pin */
IOInit(rxIntIO, OWNER_RX_SPI_CS, 0);
#ifdef STM32F7
EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler);
@@ -79,20 +75,19 @@ void A7105Init (uint32_t id)
A7105WriteID(id);
}
-
void A7105Config (const uint8_t *regsTable, uint8_t size)
{
- if (regsTable)
- {
+ if (regsTable) {
uint32_t timeout = 1000;
- for (uint8_t i = 0; i < size; i++)
- {
- if (regsTable[i] != 0xFF) { A7105WriteReg ((A7105Reg_t)i, regsTable[i]); }
+ for (uint8_t i = 0; i < size; i++) {
+ if (regsTable[i] != 0xFF) {A7105WriteReg ((A7105Reg_t)i, regsTable[i]);}
}
+
A7105Strobe(A7105_STANDBY);
A7105WriteReg(A7105_02_CALC, 0x01);
+
while ((A7105ReadReg(A7105_02_CALC) != 0) || timeout--) {}
A7105ReadReg(A7105_22_IF_CALIB_I);
@@ -103,58 +98,54 @@ void A7105Config (const uint8_t *regsTable, uint8_t size)
}
}
-
-bool A7105RxTxFinished (uint32_t *timeStamp)
-{
+bool A7105RxTxFinished (uint32_t *timeStamp) {
bool result = false;
- if (occurEvent)
- {
- if (timeStamp) *timeStamp = timeEvent;
+
+ if (occurEvent) {
+ if (timeStamp) {
+ *timeStamp = timeEvent;
+ }
+
occurEvent = false;
result = true;
}
return result;
}
-
void A7105SoftReset (void)
{
rxSpiWriteCommand((uint8_t)A7105_00_MODE, 0x00);
}
-
uint8_t A7105ReadReg (A7105Reg_t reg)
{
return rxSpiReadCommand((uint8_t)reg | 0x40, 0xFF);
}
-
void A7105WriteReg (A7105Reg_t reg, uint8_t data)
{
rxSpiWriteCommand((uint8_t)reg, data);
}
-
void A7105Strobe (A7105State_t state)
{
- if (A7105_TX == state || A7105_RX == state)
- {
+ if (A7105_TX == state || A7105_RX == state) {
EXTIEnable(rxIntIO, true);
- }
- else
- {
+ } else {
EXTIEnable(rxIntIO, false);
}
+
#ifdef RX_PA_TXEN_PIN
- if (A7105_TX == state)
- IOHi(txEnIO); /* enable PA */
- else
- IOLo(txEnIO); /* disable PA */
+ if (A7105_TX == state) {
+ IOHi(txEnIO); /* enable PA */
+ } else {
+ IOLo(txEnIO); /* disable PA */
+ }
#endif
+
rxSpiWriteByte((uint8_t)state);
}
-
void A7105WriteID(uint32_t id)
{
uint8_t data[4];
@@ -165,37 +156,37 @@ void A7105WriteID(uint32_t id)
rxSpiWriteCommandMulti((uint8_t)A7105_06_ID_DATA, &data[0], sizeof(data));
}
-
uint32_t A7105ReadID (void)
{
uint32_t id;
uint8_t data[4];
rxSpiReadCommandMulti ( (uint8_t)A7105_06_ID_DATA | 0x40, 0xFF, &data[0], sizeof(data));
- id = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3] << 0;
+ id = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3] << 0;
return id;
}
-
void A7105ReadFIFO (uint8_t *data, uint8_t num)
{
- if (data)
- {
- if(num > 64) num = 64;
- A7105Strobe(A7105_RST_RDPTR); /* reset read pointer */
+ if (data) {
+ if(num > 64){
+ num = 64;
+ }
+
+ A7105Strobe(A7105_RST_RDPTR); /* reset read pointer */
rxSpiReadCommandMulti((uint8_t)A7105_05_FIFO_DATA | 0x40, 0xFF, data, num);
}
}
-
void A7105WriteFIFO (uint8_t *data, uint8_t num)
{
- if (data)
- {
- if(num > 64) num = 64;
- A7105Strobe(A7105_RST_WRPTR); /* reset write pointer */
+ if (data) {
+ if(num > 64) {
+ num = 64;
+ }
+
+ A7105Strobe(A7105_RST_WRPTR); /* reset write pointer */
rxSpiWriteCommandMulti((uint8_t)A7105_05_FIFO_DATA, data, num);
}
}
-
#endif /* USE_RX_FLYSKY */
diff --git a/src/main/drivers/rx_a7105.h b/src/main/drivers/rx_a7105.h
index a4e43e73d6..eb9c57b800 100644
--- a/src/main/drivers/rx_a7105.h
+++ b/src/main/drivers/rx_a7105.h
@@ -1,121 +1,111 @@
/*
-* 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 .
-*/
-
-#ifndef _RX_A7105_
-#define _RX_A7105_
+ * 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
/* A7105 states for strobe */
-typedef enum
-{
- A7105_SLEEP = 0x80,
- A7105_IDLE = 0x90,
- A7105_STANDBY = 0xA0,
- A7105_PLL = 0xB0,
- A7105_RX = 0xC0,
- A7105_TX = 0xD0,
+typedef enum {
+ A7105_SLEEP = 0x80,
+ A7105_IDLE = 0x90,
+ A7105_STANDBY = 0xA0,
+ A7105_PLL = 0xB0,
+ A7105_RX = 0xC0,
+ A7105_TX = 0xD0,
A7105_RST_WRPTR = 0xE0,
A7105_RST_RDPTR = 0xF0
} A7105State_t;
-
/* Register addresses */
-typedef enum
-{
- A7105_00_MODE = 0x00,
+typedef enum {
+ A7105_00_MODE = 0x00,
A7105_01_MODE_CONTROL = 0x01,
- A7105_02_CALC = 0x02,
- A7105_03_FIFOI = 0x03,
- A7105_04_FIFOII = 0x04,
- A7105_05_FIFO_DATA = 0x05,
- A7105_06_ID_DATA = 0x06,
- A7105_07_RC_OSC_I = 0x07,
- A7105_08_RC_OSC_II = 0x08,
- A7105_09_RC_OSC_III = 0x09,
- A7105_0A_CK0_PIN = 0x0A,
- A7105_0B_GPIO1_PIN_I = 0x0B,
+ A7105_02_CALC = 0x02,
+ A7105_03_FIFOI = 0x03,
+ A7105_04_FIFOII = 0x04,
+ A7105_05_FIFO_DATA = 0x05,
+ A7105_06_ID_DATA = 0x06,
+ A7105_07_RC_OSC_I = 0x07,
+ A7105_08_RC_OSC_II = 0x08,
+ A7105_09_RC_OSC_III = 0x09,
+ A7105_0A_CK0_PIN = 0x0A,
+ A7105_0B_GPIO1_PIN_I = 0x0B,
A7105_0C_GPIO2_PIN_II = 0x0C,
- A7105_0D_CLOCK = 0x0D,
- A7105_0E_DATA_RATE = 0x0E,
- A7105_0F_PLL_I = 0x0F,
- A7105_0F_CHANNEL = 0x0F,
- A7105_10_PLL_II = 0x10,
- A7105_11_PLL_III = 0x11,
- A7105_12_PLL_IV = 0x12,
- A7105_13_PLL_V = 0x13,
- A7105_14_TX_I = 0x14,
- A7105_15_TX_II = 0x15,
- A7105_16_DELAY_I = 0x16,
- A7105_17_DELAY_II = 0x17,
- A7105_18_RX = 0x18,
- A7105_19_RX_GAIN_I = 0x19,
- A7105_1A_RX_GAIN_II = 0x1A,
- A7105_1B_RX_GAIN_III = 0x1B,
- A7105_1C_RX_GAIN_IV = 0x1C,
- A7105_1D_RSSI_THOLD = 0x1D,
- A7105_1E_ADC = 0x1E,
- A7105_1F_CODE_I = 0x1F,
- A7105_20_CODE_II = 0x20,
- A7105_21_CODE_III = 0x21,
- A7105_22_IF_CALIB_I = 0x22,
- A7105_23_IF_CALIB_II = 0x23,
- A7105_24_VCO_CURCAL = 0x24,
- A7105_25_VCO_SBCAL_I = 0x25,
+ A7105_0D_CLOCK = 0x0D,
+ A7105_0E_DATA_RATE = 0x0E,
+ A7105_0F_PLL_I = 0x0F,
+ A7105_0F_CHANNEL = 0x0F,
+ A7105_10_PLL_II = 0x10,
+ A7105_11_PLL_III = 0x11,
+ A7105_12_PLL_IV = 0x12,
+ A7105_13_PLL_V = 0x13,
+ A7105_14_TX_I = 0x14,
+ A7105_15_TX_II = 0x15,
+ A7105_16_DELAY_I = 0x16,
+ A7105_17_DELAY_II = 0x17,
+ A7105_18_RX = 0x18,
+ A7105_19_RX_GAIN_I = 0x19,
+ A7105_1A_RX_GAIN_II = 0x1A,
+ A7105_1B_RX_GAIN_III = 0x1B,
+ A7105_1C_RX_GAIN_IV = 0x1C,
+ A7105_1D_RSSI_THOLD = 0x1D,
+ A7105_1E_ADC = 0x1E,
+ A7105_1F_CODE_I = 0x1F,
+ A7105_20_CODE_II = 0x20,
+ A7105_21_CODE_III = 0x21,
+ A7105_22_IF_CALIB_I = 0x22,
+ A7105_23_IF_CALIB_II = 0x23,
+ A7105_24_VCO_CURCAL = 0x24,
+ A7105_25_VCO_SBCAL_I = 0x25,
A7105_26_VCO_SBCAL_II = 0x26,
- A7105_27_BATTERY_DET = 0x27,
- A7105_28_TX_TEST = 0x28,
- A7105_29_RX_DEM_TEST_I = 0x29,
+ A7105_27_BATTERY_DET = 0x27,
+ A7105_28_TX_TEST = 0x28,
+ A7105_29_RX_DEM_TEST_I = 0x29,
A7105_2A_RX_DEM_TEST_II = 0x2A,
- A7105_2B_CPC = 0x2B,
- A7105_2C_XTAL_TEST = 0x2C,
- A7105_2D_PLL_TEST = 0x2D,
- A7105_2E_VCO_TEST_I = 0x2E,
- A7105_2F_VCO_TEST_II = 0x2F,
- A7105_30_IFAT = 0x30,
- A7105_31_RSCALE = 0x31,
- A7105_32_FILTER_TEST = 0x32,
+ A7105_2B_CPC = 0x2B,
+ A7105_2C_XTAL_TEST = 0x2C,
+ A7105_2D_PLL_TEST = 0x2D,
+ A7105_2E_VCO_TEST_I = 0x2E,
+ A7105_2F_VCO_TEST_II = 0x2F,
+ A7105_30_IFAT = 0x30,
+ A7105_31_RSCALE = 0x31,
+ A7105_32_FILTER_TEST = 0x32,
} A7105Reg_t;
-
/* Register: A7105_00_MODE */
-#define A7105_MODE_FECF 0x40 /* [0]: FEC pass. [1]: FEC error. (FECF is read only, it is updated internally while receiving every packet.) */
-#define A7105_MODE_CRCF 0x20 /* [0]: CRC pass. [1]: CRC error. (CRCF is read only, it is updated internally while receiving every packet.) */
-#define A7105_MODE_CER 0x10 /* [0]: RF chip is disabled. [1]: RF chip is enabled. */
-#define A7105_MODE_XER 0x08 /* [0]: Crystal oscillator is disabled. [1]: Crystal oscillator is enabled. */
-#define A7105_MODE_PLLER 0x04 /* [0]: PLL is disabled. [1]: PLL is enabled. */
-#define A7105_MODE_TRSR 0x02 /* [0]: RX state. [1]: TX state. Serviceable if TRER=1 (TRX is enable). */
-#define A7105_MODE_TRER 0x01 /* [0]: TRX is disabled. [1]: TRX is enabled. */
+#define A7105_MODE_FECF 0x40 // [0]: FEC pass. [1]: FEC error. (FECF is read only, it is updated internally while receiving every packet.)
+#define A7105_MODE_CRCF 0x20 // [0]: CRC pass. [1]: CRC error. (CRCF is read only, it is updated internally while receiving every packet.)
+#define A7105_MODE_CER 0x10 // [0]: RF chip is disabled. [1]: RF chip is enabled.
+#define A7105_MODE_XER 0x08 // [0]: Crystal oscillator is disabled. [1]: Crystal oscillator is enabled.
+#define A7105_MODE_PLLER 0x04 // [0]: PLL is disabled. [1]: PLL is enabled.
+#define A7105_MODE_TRSR 0x02 // [0]: RX state. [1]: TX state. Serviceable if TRER=1 (TRX is enable).
+#define A7105_MODE_TRER 0x01 // [0]: TRX is disabled. [1]: TRX is enabled.
+void A7105Init(uint32_t id);
+void A7105SoftReset(void);
+void A7105Config(const uint8_t *regsTable, uint8_t size);
-void A7105Init (uint32_t id);
-void A7105SoftReset (void);
-void A7105Config (const uint8_t *regsTable, uint8_t size);
-
-uint8_t A7105ReadReg (A7105Reg_t reg);
-void A7105WriteReg (A7105Reg_t reg, uint8_t data);
-void A7105Strobe (A7105State_t state);
+uint8_t A7105ReadReg(A7105Reg_t reg);
+void A7105WriteReg(A7105Reg_t reg, uint8_t data);
+void A7105Strobe(A7105State_t state);
void A7105WriteID(uint32_t id);
-uint32_t A7105ReadID (void);
+uint32_t A7105ReadID(void);
-void A7105ReadFIFO (uint8_t *data, uint8_t num);
-void A7105WriteFIFO (uint8_t *data, uint8_t num);
+void A7105ReadFIFO(uint8_t *data, uint8_t num);
+void A7105WriteFIFO(uint8_t *data, uint8_t num);
-bool A7105RxTxFinished (uint32_t *timeStamp);
-
-
-#endif /* _RX_A7105_ */
+bool A7105RxTxFinished(uint32_t *timeStamp);
diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c
index d94b923500..d3bada92f8 100644
--- a/src/main/rx/flysky.c
+++ b/src/main/rx/flysky.c
@@ -1,19 +1,19 @@
/*
-* 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 .
-*/
+ * 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 "platform.h"
@@ -41,22 +41,18 @@
#include "sensors/battery.h"
-
-#if FLYSKY_CHANNEL_COUNT > 8
+#if FLYSKY_CHANNEL_COUNT > MAX_FLYSKY_CHANNEL_COUNT
#error "FlySky AFHDS protocol support 8 channel max"
#endif
-#if FLYSKY_2A_CHANNEL_COUNT > 14
+#if FLYSKY_2A_CHANNEL_COUNT > MAX_FLYSKY_2A_CHANNEL_COUNT
#error "FlySky AFHDS 2A protocol support 14 channel max"
#endif
-
PG_REGISTER_WITH_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, PG_FLYSKY_CONFIG, 0);
PG_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, .txId = 0, .rfChannelMap = {0}, .protocol = 0);
-
-static const uint8_t flySkyRegs[] =
-{
+static const uint8_t flySkyRegs[] = {
0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff, 0x00,
0x00, 0x00, 0x00, 0x03, 0x19, 0x05, 0x00, 0x50,
0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00,
@@ -66,9 +62,7 @@ static const uint8_t flySkyRegs[] =
0x01, 0x0f
};
-
-static const uint8_t flySky2ARegs[] =
-{
+static const uint8_t flySky2ARegs[] = {
0xff, 0x62, 0x00, 0x25, 0x00, 0xff, 0xff, 0x00,
0x00, 0x00, 0x00, 0x03, 0x19, 0x05, 0x00, 0x50,
0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x4f,
@@ -78,37 +72,31 @@ static const uint8_t flySky2ARegs[] =
0x01, 0x0f
};
-
-static const uint8_t flySky2ABindChannels[] =
-{
+static const uint8_t flySky2ABindChannels[] = {
0x0D, 0x8C
};
-
-static const uint8_t flySkyRfChannels[16][16] =
-{
- {0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0},
- {0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a},
- {0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82},
- {0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a},
- {0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96},
- {0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28},
- {0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64},
- {0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50},
- {0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64},
- {0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50},
- {0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
- {0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46},
- {0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82},
- {0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46},
- {0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
- {0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46}
+static const uint8_t flySkyRfChannels[16][16] = {
+ { 0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0},
+ { 0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a},
+ { 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82},
+ { 0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a},
+ { 0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96},
+ { 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28},
+ { 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64},
+ { 0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50},
+ { 0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64},
+ { 0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50},
+ { 0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
+ { 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46},
+ { 0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82},
+ { 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46},
+ { 0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
+ { 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46}
};
-
-const timings_t flySkyTimings = { .packet = 1500, .firstPacket = 1900, .syncPacket = 2250, .telemetry = 0xFFFFFFFF };
-const timings_t flySky2ATimings = { .packet = 3850, .firstPacket = 4850, .syncPacket = 5775, .telemetry = 57000 };
-
+const timings_t flySkyTimings = {.packet = 1500, .firstPacket = 1900, .syncPacket = 2250, .telemetry = 0xFFFFFFFF};
+const timings_t flySky2ATimings = {.packet = 3850, .firstPacket = 4850, .syncPacket = 5775, .telemetry = 57000};
static rx_spi_protocol_e protocol = RX_SPI_A7105_FLYSKY_2A;
static const timings_t *timings = &flySky2ATimings;
@@ -118,259 +106,109 @@ static uint32_t countTimeout = 0;
static uint32_t countPacket = 0;
static uint32_t txId = 0;
static uint32_t rxId = 0;
-static bool binded = false;
+static bool bound = false;
static bool sendTelemetry = false;
static uint16_t errorRate = 0;
static uint16_t rssi_dBm = 0;
-static uint8_t rfChannelMap[NUMFREQ];
+static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT];
-static inline rx_spi_received_e flySkyReadAndProcess (uint8_t *payload, const uint32_t timeStamp);
-static inline rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_t timeStamp);
-static inline void flySkyCalculateRfChannels (void);
-static inline bool isValidPacket (const uint8_t *packet);
-static inline void buildAndWriteTelemetry (uint8_t *packet);
-static inline uint8_t getNextChannel (uint8_t step);
-static void checkRSSI (void);
-static void resetTimeout (const uint32_t timeStamp);
-static inline void checkTimeout (void);
-
-void flySkyInit (const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig)
+static uint8_t getNextChannel (uint8_t step)
{
- protocol = rxConfig->rx_spi_protocol;
- if (protocol != flySkyConfig()->protocol) { PG_RESET(flySkyConfig); }
-
- IO_t bindIO = IOGetByTag(IO_TAG(RX_FLYSKY_BIND_PIN));
- IOInit(bindIO, OWNER_RX_SPI_CS, 0);
- IOConfigGPIO(bindIO, IOCFG_IPU);
-
- uint8_t startRxChannel;
-
- if (protocol == RX_SPI_A7105_FLYSKY_2A)
- {
- rxRuntimeConfig->channelCount = FLYSKY_2A_CHANNEL_COUNT;
- timings = &flySky2ATimings;
- rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
- startRxChannel = flySky2ABindChannels[0];
- A7105Init(0x5475c52A);
- A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
- }
- else
- {
- rxRuntimeConfig->channelCount = FLYSKY_CHANNEL_COUNT;
- timings = &flySkyTimings;
- startRxChannel = 0;
- A7105Init(0x5475c52A);
- A7105Config(flySkyRegs, sizeof(flySkyRegs));
- }
-
- if ( !IORead(bindIO) || flySkyConfig()->txId == 0)
- {
- binded = false;
- }
- else
- {
- binded = true;
- txId = flySkyConfig()->txId; /* load TXID */
- memcpy (rfChannelMap, flySkyConfig()->rfChannelMap, NUMFREQ); /* load channel map */
- startRxChannel = getNextChannel(0);
- }
-
- A7105WriteReg(A7105_0F_CHANNEL, startRxChannel);
- A7105Strobe(A7105_RX); /* start listening */
-
- resetTimeout(micros());
+ static uint8_t channel = 0;
+ channel = (channel + step) & 0x0F;
+ return rfChannelMap[channel];
}
-
-void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+static void flySkyCalculateRfChannels (void)
{
- if (rcData && payload)
- {
- uint32_t channelCount;
- channelCount = (protocol == RX_SPI_A7105_FLYSKY_2A) ? (FLYSKY_2A_CHANNEL_COUNT) : (FLYSKY_CHANNEL_COUNT);
- for (uint8_t i = 0; i < channelCount; i++)
- {
- rcData[i] = payload[2 * i + 1] << 8 | payload [2 * i + 0];
+ uint32_t channelRow = txId & 0x0F;
+ uint32_t channelOffset = ((txId & 0xF0) >> 4) + 1;
+
+ if (channelOffset > 9) {
+ channelOffset = 9; // from sloped soarer findings, bug in flysky protocol
+ }
+
+ for (uint32_t i = 0; i < FLYSKY_FREQUENCY_COUNT; i++) {
+ rfChannelMap[i] = flySkyRfChannels[channelRow][i] - channelOffset;
+ }
+}
+
+static void resetTimeout (const uint32_t timeStamp)
+{
+ timeLastPacket = timeStamp;
+ timeout = timings->firstPacket;
+ countTimeout = 0;
+ countPacket++;
+}
+
+static void checkTimeout (void)
+{
+ static uint32_t timeMeasuareErrRate = 0;
+ static uint32_t timeLastTelemetry = 0;
+ uint32_t time = micros();
+
+ if ((time - timeMeasuareErrRate) > (100 * timings->packet)) {
+ if (countPacket > 100) {
+ countPacket = 100;
+ }
+ errorRate = 100 - countPacket;
+ countPacket = 0;
+ timeMeasuareErrRate = time;
+ }
+
+ if ((time - timeLastTelemetry) > timings->telemetry) {
+ timeLastTelemetry = time;
+ sendTelemetry = true;
+ }
+
+ if ((time - timeLastPacket) > timeout) {
+ uint32_t stepOver = (time - timeLastPacket) / timings->packet;
+
+ timeLastPacket = (stepOver > 1) ? (time) : (timeLastPacket + timeout);
+
+ A7105Strobe(A7105_STANDBY);
+ A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(stepOver % FLYSKY_FREQUENCY_COUNT));
+ A7105Strobe(A7105_RX);
+
+ if(countTimeout > 31) {
+ timeout = timings->syncPacket;
+ rssi = 0;
+ } else {
+ timeout = timings->packet;
+ countTimeout++;
}
}
}
-
-rx_spi_received_e flySkyDataReceived (uint8_t *payload)
+static void checkRSSI (void)
{
- rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
- uint32_t timeStamp;
+ static uint8_t buf[FLYSKY_RSSI_SAMPLE_COUNT] = {0};
+ static int16_t sum = 0;
+ static uint16_t currentIndex = 0;
- if (A7105RxTxFinished(&timeStamp))
- {
- uint8_t modeReg = A7105ReadReg(A7105_00_MODE);
+ uint8_t adcValue = A7105ReadReg(A7105_1D_RSSI_THOLD);
- if (((modeReg & A7105_MODE_TRSR) != 0) && ((modeReg & A7105_MODE_TRER) == 0)) /* TX complete */
- {
- if (binded) { A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1)); }
- A7105Strobe(A7105_RX);
- }
- else if ((modeReg & (A7105_MODE_CRCF|A7105_MODE_TRER)) == 0) /* RX complete, CRC pass */
- {
- if (protocol == RX_SPI_A7105_FLYSKY_2A)
- {
- result = flySky2AReadAndProcess(payload, timeStamp);
- }
- else
- {
- result = flySkyReadAndProcess(payload, timeStamp);
- }
- }
- else
- {
- A7105Strobe(A7105_RX);
- }
- }
+ sum += adcValue;
+ sum -= buf[currentIndex];
+ buf[currentIndex] = adcValue;
+ currentIndex = (currentIndex + 1) % FLYSKY_RSSI_SAMPLE_COUNT;
- if (binded) { checkTimeout(); }
+ rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm
- if (result == RX_SPI_RECEIVED_BIND)
- {
- flySkyConfigMutable()->txId = txId; /* store TXID */
- memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, NUMFREQ);/* store channel map */
- flySkyConfigMutable()->protocol = protocol;
- writeEEPROM();
- }
-
- return result;
+ int16_t tmp = 2280 - 24 * rssi_dBm;// convert to [0...1023]
+ rssi = (uint16_t) constrain(tmp, 0, 1023);// external variable from "rx/rx.h"
}
-
-static inline rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_t timeStamp)
-{
- rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
- A7105State_t newState = A7105_RX;
- uint8_t packet[FLYSKY_2A_PAYLOAD_SIZE];
-
- uint8_t bytesToRead = (binded) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + NUMFREQ);
- A7105ReadFIFO(packet, bytesToRead);
-
- switch (packet[0])
- {
- case FLYSKY_2A_PACKET_RC_DATA:
- case FLYSKY_2A_PACKET_FS_SETTINGS: /* failsafe settings */
- case FLYSKY_2A_PACKET_SETTINGS: /* receiver settings */
- if (isValidPacket(packet))
- {
- checkRSSI();
- resetTimeout(timeStamp);
-
- const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
- if (rcPacket->type == FLYSKY_2A_PACKET_RC_DATA)
- {
- if (payload) memcpy(payload, rcPacket->data, 2*FLYSKY_2A_CHANNEL_COUNT);
-
- if (sendTelemetry)
- {
- buildAndWriteTelemetry(packet);
- newState = A7105_TX;
- sendTelemetry = false;
- }
-
- result = RX_SPI_RECEIVED_DATA;
- }
-
- if (newState != A7105_TX)
- {
- A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
- }
- }
- break;
-
- case FLYSKY_2A_PACKET_BIND1:
- case FLYSKY_2A_PACKET_BIND2:
- if (!binded)
- {
- resetTimeout(timeStamp);
-
- flySky2ABindPkt_t *bindPacket = (flySky2ABindPkt_t*) packet;
-
- if (bindPacket->rfChannelMap[0] != 0xFF)
- {
- memcpy(rfChannelMap, bindPacket->rfChannelMap, NUMFREQ); /* get TX channels */
- }
-
- txId = bindPacket->txId;
- bindPacket->rxId = rxId;
- memset(bindPacket->rfChannelMap, 0xFF, 26); /* erase channelMap and 10 bytes after it */
-
- binded = ((bindPacket->state != 0) && (bindPacket->state != 1));
- if (binded) { result = RX_SPI_RECEIVED_BIND; } /* bind complete */
-
- A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE);
-
- newState = A7105_TX;
- }
- break;
-
- default:
- break;
- }
-
- A7105Strobe(newState);
- return result;
-}
-
-
-static inline rx_spi_received_e flySkyReadAndProcess (uint8_t *payload, const uint32_t timeStamp)
-{
- rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
- uint8_t packet[FLYSKY_PAYLOAD_SIZE];
-
- uint8_t bytesToRead = (binded) ? (5 + 2*FLYSKY_CHANNEL_COUNT) : (5);
- A7105ReadFIFO(packet, bytesToRead);
-
- const flySkyRcDataPkt_t *rcPacket = (const flySkyRcDataPkt_t*) packet;
-
- if (binded && rcPacket->type == FLYSKY_PACKET_RC_DATA && rcPacket->txId == txId)
- {
- checkRSSI();
- resetTimeout(timeStamp);
-
- if (payload) { memcpy(payload, rcPacket->data, 2*FLYSKY_CHANNEL_COUNT); }
-
- A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
-
- result = RX_SPI_RECEIVED_DATA;
- }
-
- if (!binded && rcPacket->type == FLYSKY_PACKET_BIND)
- {
- resetTimeout(timeStamp);
-
- txId = rcPacket->txId;
- flySkyCalculateRfChannels();
-
- A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(0));
-
- binded = true;
-
- result = RX_SPI_RECEIVED_BIND;
- }
-
- A7105Strobe(A7105_RX);
- return result;
-}
-
-
-static inline bool isValidPacket (const uint8_t *packet)
-{
+static bool isValidPacket (const uint8_t *packet) {
const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
return (rcPacket->rxId == rxId && rcPacket->txId == txId);
}
-
-static inline void buildAndWriteTelemetry (uint8_t *packet)
+static void buildAndWriteTelemetry (uint8_t *packet)
{
- if (packet)
- {
- static uint8_t bytesToWrite = FLYSKY_2A_PAYLOAD_SIZE; /* first time write full packet to buffer a7105 */
+ if (packet) {
+ static uint8_t bytesToWrite = FLYSKY_2A_PAYLOAD_SIZE; // first time write full packet to buffer a7105
flySky2ATelemetryPkt_t *telemertyPacket = (flySky2ATelemetryPkt_t*) packet;
uint16_t voltage = 10 * getBatteryVoltage();
@@ -395,102 +233,216 @@ static inline void buildAndWriteTelemetry (uint8_t *packet)
A7105WriteFIFO(packet, bytesToWrite);
- bytesToWrite = 9 + 3 * sizeof(flySky2ASens_t); /* next time write only bytes that could change, the others are already set as 0xFF in buffer a7105 */
+ bytesToWrite = 9 + 3 * sizeof(flySky2ASens_t);// next time write only bytes that could change, the others are already set as 0xFF in buffer a7105
}
}
-
-static void checkRSSI (void)
+static rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_t timeStamp)
{
- static uint8_t buf[RSSI_SAMPLE_COUNT] = {0};
- static int16_t sum = 0;
- static uint16_t currentIndex = 0;
+ rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
+ A7105State_t newState = A7105_RX;
+ uint8_t packet[FLYSKY_2A_PAYLOAD_SIZE];
- uint8_t adcValue = A7105ReadReg(A7105_1D_RSSI_THOLD);
+ uint8_t bytesToRead = (bound) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + FLYSKY_FREQUENCY_COUNT);
+ A7105ReadFIFO(packet, bytesToRead);
- sum += adcValue;
- sum -= buf[currentIndex];
- buf[currentIndex] = adcValue;
- currentIndex = (currentIndex + 1) % RSSI_SAMPLE_COUNT;
+ switch (packet[0]) {
+ case FLYSKY_2A_PACKET_RC_DATA:
+ case FLYSKY_2A_PACKET_FS_SETTINGS: // failsafe settings
+ case FLYSKY_2A_PACKET_SETTINGS: // receiver settings
+ if (isValidPacket(packet)) {
+ checkRSSI();
+ resetTimeout(timeStamp);
- rssi_dBm = 50 + sum / (3 * RSSI_SAMPLE_COUNT); /* range about [95...52], -dBm */
+ const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
- int16_t tmp = 2280 - 24 * rssi_dBm; /* convert to [0...1023] */
- rssi = (uint16_t) constrain(tmp, 0, 1023); /* external variable from "rx/rx.h" */
-}
+ if (rcPacket->type == FLYSKY_2A_PACKET_RC_DATA) {
+ if (payload) {
+ memcpy(payload, rcPacket->data, 2*FLYSKY_2A_CHANNEL_COUNT);
+ }
+ if (sendTelemetry) {
+ buildAndWriteTelemetry(packet);
+ newState = A7105_TX;
+ sendTelemetry = false;
+ }
-static inline uint8_t getNextChannel (uint8_t step)
-{
- static uint8_t channel = 0;
- channel = (channel + step) & 0x0F;
- return rfChannelMap[channel];
-}
+ result = RX_SPI_RECEIVED_DATA;
+ }
-
-static inline void flySkyCalculateRfChannels (void)
-{
- uint32_t channelRow = txId & 0x0F;
- uint32_t channelOffset = ((txId & 0xF0) >> 4) + 1;
- if (channelOffset > 9) channelOffset = 9; /* from sloped soarer findings, bug in flysky protocol */
- for (uint32_t i = 0; i < NUMFREQ; i++)
- {
- rfChannelMap[i] = flySkyRfChannels[channelRow][i] - channelOffset;
- }
-}
-
-
-static void resetTimeout (const uint32_t timeStamp)
-{
- timeLastPacket = timeStamp;
- timeout = timings->firstPacket;
- countTimeout = 0;
- countPacket++;
-}
-
-
-static inline void checkTimeout (void)
-{
- static uint32_t timeMeasuareErrRate = 0;
- static uint32_t timeLastTelemetry = 0;
- uint32_t time = micros();
-
- if ((time - timeMeasuareErrRate) > (100 * timings->packet))
- {
- if (countPacket > 100) countPacket = 100;
- errorRate = 100 - countPacket;
- countPacket = 0;
- timeMeasuareErrRate = time;
- }
-
- if ((time - timeLastTelemetry) > timings->telemetry)
- {
- timeLastTelemetry = time;
- sendTelemetry = true;
- }
-
- if ((time - timeLastPacket) > timeout)
- {
- uint32_t stepOver = (time - timeLastPacket) / timings->packet;
-
- timeLastPacket = (stepOver > 1) ? (time) : (timeLastPacket + timeout);
-
- A7105Strobe(A7105_STANDBY);
- A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(stepOver % NUMFREQ));
- A7105Strobe(A7105_RX);
-
- if(countTimeout > 31)
- {
- timeout = timings->syncPacket;
- rssi = 0;
+ if (newState != A7105_TX) {
+ A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
+ }
}
- else
- {
- timeout = timings->packet;
- countTimeout++;
+ break;
+
+ case FLYSKY_2A_PACKET_BIND1:
+ case FLYSKY_2A_PACKET_BIND2:
+ if (!bound) {
+ resetTimeout(timeStamp);
+
+ flySky2ABindPkt_t *bindPacket = (flySky2ABindPkt_t*) packet;
+
+ if (bindPacket->rfChannelMap[0] != 0xFF) {
+ memcpy(rfChannelMap, bindPacket->rfChannelMap, FLYSKY_FREQUENCY_COUNT); // get TX channels
+ }
+
+ txId = bindPacket->txId;
+ bindPacket->rxId = rxId;
+ memset(bindPacket->rfChannelMap, 0xFF, 26); // erase channelMap and 10 bytes after it
+
+ bound = ((bindPacket->state != 0) && (bindPacket->state != 1));
+
+ if (bound) { // bind complete
+ result = RX_SPI_RECEIVED_BIND;
+ }
+
+ A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE);
+
+ newState = A7105_TX;
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ A7105Strobe(newState);
+ return result;
+}
+
+static rx_spi_received_e flySkyReadAndProcess (uint8_t *payload, const uint32_t timeStamp)
+{
+ rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
+ uint8_t packet[FLYSKY_PAYLOAD_SIZE];
+
+ uint8_t bytesToRead = (bound) ? (5 + 2*FLYSKY_CHANNEL_COUNT) : (5);
+ A7105ReadFIFO(packet, bytesToRead);
+
+ const flySkyRcDataPkt_t *rcPacket = (const flySkyRcDataPkt_t*) packet;
+
+ if (bound && rcPacket->type == FLYSKY_PACKET_RC_DATA && rcPacket->txId == txId) {
+ checkRSSI();
+ resetTimeout(timeStamp);
+
+ if (payload) {
+ memcpy(payload, rcPacket->data, 2*FLYSKY_CHANNEL_COUNT);
+ }
+
+ A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
+
+ result = RX_SPI_RECEIVED_DATA;
+ }
+
+ if (!bound && rcPacket->type == FLYSKY_PACKET_BIND) {
+ resetTimeout(timeStamp);
+
+ txId = rcPacket->txId;
+ flySkyCalculateRfChannels();
+
+ A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(0));
+
+ bound = true;
+
+ result = RX_SPI_RECEIVED_BIND;
+ }
+
+ A7105Strobe(A7105_RX);
+ return result;
+}
+
+void flySkyInit (const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig)
+{
+ protocol = rxConfig->rx_spi_protocol;
+
+ if (protocol != flySkyConfig()->protocol) {
+ PG_RESET(flySkyConfig);
+ }
+
+ IO_t bindIO = IOGetByTag(IO_TAG(RX_FLYSKY_BIND_PIN));
+ IOInit(bindIO, OWNER_RX_SPI_CS, 0);
+ IOConfigGPIO(bindIO, IOCFG_IPU);
+
+ uint8_t startRxChannel;
+
+ if (protocol == RX_SPI_A7105_FLYSKY_2A) {
+ rxRuntimeConfig->channelCount = FLYSKY_2A_CHANNEL_COUNT;
+ timings = &flySky2ATimings;
+ rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
+ startRxChannel = flySky2ABindChannels[0];
+ A7105Init(0x5475c52A);
+ A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
+ } else {
+ rxRuntimeConfig->channelCount = FLYSKY_CHANNEL_COUNT;
+ timings = &flySkyTimings;
+ startRxChannel = 0;
+ A7105Init(0x5475c52A);
+ A7105Config(flySkyRegs, sizeof(flySkyRegs));
+ }
+
+ if ( !IORead(bindIO) || flySkyConfig()->txId == 0) {
+ bound = false;
+ } else {
+ bound = true;
+ txId = flySkyConfig()->txId; // load TXID
+ memcpy (rfChannelMap, flySkyConfig()->rfChannelMap, FLYSKY_FREQUENCY_COUNT);// load channel map
+ startRxChannel = getNextChannel(0);
+ }
+
+ A7105WriteReg(A7105_0F_CHANNEL, startRxChannel);
+ A7105Strobe(A7105_RX); // start listening
+
+ resetTimeout(micros());
+}
+
+void flySkySetRcDataFromPayload (uint16_t *rcData, const uint8_t *payload)
+{
+ if (rcData && payload) {
+ uint32_t channelCount;
+ channelCount = (protocol == RX_SPI_A7105_FLYSKY_2A) ? (FLYSKY_2A_CHANNEL_COUNT) : (FLYSKY_CHANNEL_COUNT);
+
+ for (uint8_t i = 0; i < channelCount; i++) {
+ rcData[i] = payload[2 * i + 1] << 8 | payload [2 * i + 0];
}
}
}
+rx_spi_received_e flySkyDataReceived (uint8_t *payload)
+{
+ rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
+ uint32_t timeStamp;
+
+ if (A7105RxTxFinished(&timeStamp)) {
+ uint8_t modeReg = A7105ReadReg(A7105_00_MODE);
+
+ if (((modeReg & A7105_MODE_TRSR) != 0) && ((modeReg & A7105_MODE_TRER) == 0)) { // TX complete
+ if (bound) {
+ A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
+ }
+ A7105Strobe(A7105_RX);
+ } else if ((modeReg & (A7105_MODE_CRCF|A7105_MODE_TRER)) == 0) { // RX complete, CRC pass
+ if (protocol == RX_SPI_A7105_FLYSKY_2A) {
+ result = flySky2AReadAndProcess(payload, timeStamp);
+ } else {
+ result = flySkyReadAndProcess(payload, timeStamp);
+ }
+ } else {
+ A7105Strobe(A7105_RX);
+ }
+ }
+
+ if (bound) {
+ checkTimeout();
+ }
+
+ if (result == RX_SPI_RECEIVED_BIND) {
+ flySkyConfigMutable()->txId = txId; // store TXID
+ memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, FLYSKY_FREQUENCY_COUNT);// store channel map
+ flySkyConfigMutable()->protocol = protocol;
+ writeEEPROM();
+ }
+
+ return result;
+}
#endif /* USE_RX_FLYSKY */
diff --git a/src/main/rx/flysky.h b/src/main/rx/flysky.h
index 0c3d7df41b..7862f01926 100644
--- a/src/main/rx/flysky.h
+++ b/src/main/rx/flysky.h
@@ -1,43 +1,36 @@
/*
-* 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 .
-*/
+ * 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 .
+ */
-#ifndef _FLYSKY_H_
-#define _FLYSKY_H_
+#pragma once
#include
#include
-
-typedef struct flySkyConfig_s
-{
+typedef struct flySkyConfig_s {
uint32_t txId;
uint8_t rfChannelMap[16];
rx_spi_protocol_e protocol;
} flySkyConfig_t;
-
PG_DECLARE(flySkyConfig_t, flySkyConfig);
-
struct rxConfig_s;
struct rxRuntimeConfig_s;
-void flySkyInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void flySkyInit(const struct rxConfig_s *rxConfig,
+ struct rxRuntimeConfig_s *rxRuntimeConfig);
void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
rx_spi_received_e flySkyDataReceived(uint8_t *payload);
-
-
-#endif /* _FLYSKY_H_ */
diff --git a/src/main/rx/flysky_defs.h b/src/main/rx/flysky_defs.h
index ae52109040..df2eeca0bc 100644
--- a/src/main/rx/flysky_defs.h
+++ b/src/main/rx/flysky_defs.h
@@ -1,120 +1,102 @@
/*
-* 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 .
-*/
+ * 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 .
+ */
-#ifndef _FLYSKY_DEFS_H_
-#define _FLYSKY_DEFS_H_
+#pragma once
#include
#include
+#define MAX_FLYSKY_CHANNEL_COUNT 8
+#define MAX_FLYSKY_2A_CHANNEL_COUNT 14
+
+#define FLYSKY_PAYLOAD_SIZE 21
+#define FLYSKY_2A_PAYLOAD_SIZE 37
+
+#define FLYSKY_FREQUENCY_COUNT 16
+#define FLYSKY_RSSI_SAMPLE_COUNT 16
#ifndef FLYSKY_CHANNEL_COUNT
-#define FLYSKY_CHANNEL_COUNT 8
+#define FLYSKY_CHANNEL_COUNT MAX_FLYSKY_CHANNEL_COUNT
#endif
#ifndef FLYSKY_2A_CHANNEL_COUNT
-#define FLYSKY_2A_CHANNEL_COUNT 14
+#define FLYSKY_2A_CHANNEL_COUNT MAX_FLYSKY_2A_CHANNEL_COUNT
#endif
-
-#define FLYSKY_PAYLOAD_SIZE 21
-#define FLYSKY_2A_PAYLOAD_SIZE 37
-#define NUMFREQ 16
-#define RSSI_SAMPLE_COUNT 16
-
-
-typedef struct __attribute__((packed))
-{
+typedef struct __attribute__((packed)) {
uint8_t type;
uint8_t number;
uint8_t valueL;
uint8_t valueH;
} flySky2ASens_t;
-
-typedef struct __attribute__((packed))
-{
- uint8_t type;
+typedef struct __attribute__((packed)) {
+ uint8_t type;
uint32_t txId;
uint32_t rxId;
flySky2ASens_t sens[7];
} flySky2ATelemetryPkt_t;
-
-typedef struct __attribute__((packed))
-{
- uint8_t type;
+typedef struct __attribute__((packed)) {
+ uint8_t type;
uint32_t txId;
uint32_t rxId;
- uint8_t state;
- uint8_t reserved1;
- uint8_t rfChannelMap[16];
- uint8_t reserved2[10];
+ uint8_t state;
+ uint8_t reserved1;
+ uint8_t rfChannelMap[16];
+ uint8_t reserved2[10];
} flySky2ABindPkt_t;
-
-typedef struct __attribute__((packed))
-{
- uint8_t type;
+typedef struct __attribute__((packed)) {
+ uint8_t type;
uint32_t txId;
uint32_t rxId;
- uint8_t data[28];
+ uint8_t data[28];
} flySky2ARcDataPkt_t;
-
-typedef struct __attribute__((packed))
-{
- uint8_t type;
+typedef struct __attribute__((packed)) {
+ uint8_t type;
uint32_t txId;
- uint8_t data[16];
+ uint8_t data[16];
} flySkyRcDataPkt_t;
-
-typedef struct
-{
+typedef struct {
uint32_t packet;
uint32_t firstPacket;
uint32_t syncPacket;
uint32_t telemetry;
} timings_t;
-
-enum
-{
- SENSOR_INT_V = 0x00,
- SENSOR_TEMP = 0x01,
- SENSOR_MOT_RPM = 0x02,
- SENSOR_EXT_V = 0x03,
- SENSOR_RSSI = 0xFC,
- SENSOR_ERR_RATE = 0xFE
+enum {
+ SENSOR_INT_V = 0x00,
+ SENSOR_TEMP = 0x01,
+ SENSOR_MOT_RPM = 0x02,
+ SENSOR_EXT_V = 0x03,
+ SENSOR_RSSI = 0xFC,
+ SENSOR_ERR_RATE = 0xFE
};
-
-enum
-{
- FLYSKY_2A_PACKET_RC_DATA = 0x58,
- FLYSKY_2A_PACKET_BIND1 = 0xBB,
- FLYSKY_2A_PACKET_BIND2 = 0xBC,
- FLYSKY_2A_PACKET_FS_SETTINGS = 0x56,
- FLYSKY_2A_PACKET_SETTINGS = 0xAA,
- FLYSKY_2A_PACKET_TELEMETRY = 0xAA,
- FLYSKY_PACKET_RC_DATA = 0x55,
- FLYSKY_PACKET_BIND = 0xAA
+enum {
+ FLYSKY_2A_PACKET_RC_DATA = 0x58,
+ FLYSKY_2A_PACKET_BIND1 = 0xBB,
+ FLYSKY_2A_PACKET_BIND2 = 0xBC,
+ FLYSKY_2A_PACKET_FS_SETTINGS = 0x56,
+ FLYSKY_2A_PACKET_SETTINGS = 0xAA,
+ FLYSKY_2A_PACKET_TELEMETRY = 0xAA,
+ FLYSKY_PACKET_RC_DATA = 0x55,
+ FLYSKY_PACKET_BIND = 0xAA
};
-
-
-#endif /* _FLYSKY_DEFS_H_ */
diff --git a/src/main/target/EACHIF3/target.h b/src/main/target/EACHIF3/target.h
index 7a8abd18b8..79f2bbb3d3 100644
--- a/src/main/target/EACHIF3/target.h
+++ b/src/main/target/EACHIF3/target.h
@@ -49,7 +49,6 @@
#define USE_RX_SPI
-#define USE_RX_A7105
#define USE_RX_FLYSKY
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A
#define FLYSKY_2A_CHANNEL_COUNT 10