From ab5c71b84f869a84fbe97bb60e1484a489157f1c Mon Sep 17 00:00:00 2001 From: "e.vladisenko" Date: Thu, 7 Sep 2017 20:16:18 +0300 Subject: [PATCH] Change code style to the K&R. Delete "USE_RX_A7105" macro. --- src/main/drivers/rx_a7105.c | 117 +++--- src/main/drivers/rx_a7105.h | 188 ++++----- src/main/rx/flysky.c | 672 ++++++++++++++----------------- src/main/rx/flysky.h | 45 +-- src/main/rx/flysky_defs.h | 134 +++--- src/main/target/EACHIF3/target.h | 1 - 6 files changed, 532 insertions(+), 625 deletions(-) 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