From 4a0c89baf722135accf152712278db8ef7fc51c0 Mon Sep 17 00:00:00 2001 From: "e.vladisenko" Date: Wed, 6 Sep 2017 19:49:36 +0300 Subject: [PATCH 1/3] Add support FlySky receiver over SPI and "EachiWhoop" target --- src/main/config/parameter_group_ids.h | 3 +- src/main/drivers/rx_a7105.c | 201 +++++++++++ src/main/drivers/rx_a7105.h | 121 +++++++ src/main/fc/settings.c | 4 +- src/main/rx/flysky.c | 496 ++++++++++++++++++++++++++ src/main/rx/flysky.h | 43 +++ src/main/rx/flysky_defs.h | 120 +++++++ src/main/rx/rx_spi.c | 10 + src/main/rx/rx_spi.h | 2 + src/main/target/EACHIF3/target.c | 34 ++ src/main/target/EACHIF3/target.h | 127 +++++++ src/main/target/EACHIF3/target.mk | 11 + 12 files changed, 1170 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/rx_a7105.c create mode 100644 src/main/drivers/rx_a7105.h create mode 100644 src/main/rx/flysky.c create mode 100644 src/main/rx/flysky.h create mode 100644 src/main/rx/flysky_defs.h create mode 100644 src/main/target/EACHIF3/target.c create mode 100644 src/main/target/EACHIF3/target.h create mode 100644 src/main/target/EACHIF3/target.mk diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index ea95a6a80e..20c7578003 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -114,7 +114,8 @@ #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 #define PG_FRSKY_D_CONFIG 523 -#define PG_BETAFLIGHT_END 523 +#define PG_FLYSKY_CONFIG 524 +#define PG_BETAFLIGHT_END 524 // OSD configuration (subject to change) diff --git a/src/main/drivers/rx_a7105.c b/src/main/drivers/rx_a7105.c new file mode 100644 index 0000000000..ecef155c43 --- /dev/null +++ b/src/main/drivers/rx_a7105.c @@ -0,0 +1,201 @@ +/* +* 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 + +#ifdef USE_RX_A7105 + +#include "drivers/rx_a7105.h" +#include "drivers/bus_spi.h" +#include "drivers/rx_spi.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/exti.h" +#include "drivers/time.h" + + +#ifdef RX_PA_TXEN_PIN +static IO_t txEnIO = IO_NONE; +#endif + +static IO_t rxIntIO = IO_NONE; +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) + { + timeEvent = micros(); + occurEvent = true; + } +} + + +void A7105Init (uint32_t id) +{ + spiDeviceByInstance(RX_SPI_INSTANCE); + rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); /* config receiver IRQ pin */ + IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); +#ifdef STM32F7 + EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); + EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_PULLDOWN)); +#else + IOConfigGPIO(rxIntIO, IOCFG_IPD); + EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); + EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); +#endif + EXTIEnable(rxIntIO, false); + +#ifdef RX_PA_TXEN_PIN + txEnIO = IOGetByTag(IO_TAG(RX_PA_TXEN_PIN)); + IOInit(txEnIO, OWNER_RX_SPI_CS, 0); + IOConfigGPIO(txEnIO, IOCFG_OUT_PP); +#endif + + A7105SoftReset(); + A7105WriteID(id); +} + + +void A7105Config (const uint8_t *regsTable, uint8_t size) +{ + if (regsTable) + { + uint32_t timeout = 1000; + + 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); + + A7105WriteReg(A7105_24_VCO_CURCAL, 0x13); + A7105WriteReg(A7105_25_VCO_SBCAL_I, 0x09); + A7105Strobe(A7105_STANDBY); + } +} + + +bool A7105RxTxFinished (uint32_t *timeStamp) +{ + bool result = false; + 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) + { + EXTIEnable(rxIntIO, true); + } + else + { + EXTIEnable(rxIntIO, false); + } +#ifdef RX_PA_TXEN_PIN + 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]; + data[0] = (id >> 24) & 0xFF; + data[1] = (id >> 16) & 0xFF; + data[2] = (id >> 8) & 0xFF; + data[3] = (id >> 0) & 0xFF; + 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; + return id; +} + + +void A7105ReadFIFO (uint8_t *data, uint8_t num) +{ + 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 */ + 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 new file mode 100644 index 0000000000..a4e43e73d6 --- /dev/null +++ b/src/main/drivers/rx_a7105.h @@ -0,0 +1,121 @@ +/* +* 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_ + + +/* A7105 states for strobe */ +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, + 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_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_26_VCO_SBCAL_II = 0x26, + 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, +} 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. */ + + +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); + +void A7105WriteID(uint32_t id); +uint32_t A7105ReadID (void); + +void A7105ReadFIFO (uint8_t *data, uint8_t num); +void A7105WriteFIFO (uint8_t *data, uint8_t num); + +bool A7105RxTxFinished (uint32_t *timeStamp); + + +#endif /* _RX_A7105_ */ diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 20b9132b67..5fa30ab16b 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -184,7 +184,9 @@ static const char * const lookupTableRxSpi[] = { "CX10A", "H8_3D", "INAV", - "FRSKY_D" + "FRSKY_D", + "FLYSKY", + "FLYSKY_2A" }; #endif diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c new file mode 100644 index 0000000000..d94b923500 --- /dev/null +++ b/src/main/rx/flysky.c @@ -0,0 +1,496 @@ +/* +* 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" + +#ifdef USE_RX_FLYSKY + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/utils.h" +#include "config/config_eeprom.h" +#include "config/parameter_group_ids.h" +#include "fc/config.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" +#include "rx/flysky.h" +#include "rx/flysky_defs.h" + +#include "drivers/rx_a7105.h" +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/io.h" + +#include "sensors/battery.h" + + +#if FLYSKY_CHANNEL_COUNT > 8 +#error "FlySky AFHDS protocol support 8 channel max" +#endif + +#if FLYSKY_2A_CHANNEL_COUNT > 14 +#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[] = +{ + 0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x03, 0x19, 0x05, 0x00, 0x50, + 0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00, + 0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f, + 0x13, 0xc3, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00, + 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00, + 0x01, 0x0f +}; + + +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, + 0x62, 0x80, 0xff, 0xff, 0x2a, 0x32, 0xc3, 0x1f, + 0x1e, 0xff, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00, + 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00, + 0x01, 0x0f +}; + + +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} +}; + + +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; +static uint32_t timeout = 0; +static uint32_t timeLastPacket = 0; +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 sendTelemetry = false; +static uint16_t errorRate = 0; +static uint16_t rssi_dBm = 0; +static uint8_t rfChannelMap[NUMFREQ]; + + +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) +{ + 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()); +} + + +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 (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); + } + } + + if (binded) { checkTimeout(); } + + 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; +} + + +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) +{ + const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet; + return (rcPacket->rxId == rxId && rcPacket->txId == txId); +} + + +static inline void buildAndWriteTelemetry (uint8_t *packet) +{ + 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(); + + telemertyPacket->type = FLYSKY_2A_PACKET_TELEMETRY; + + telemertyPacket->sens[0].type = SENSOR_INT_V; + telemertyPacket->sens[0].number = 0; + telemertyPacket->sens[0].valueL = voltage & 0xFF; + telemertyPacket->sens[0].valueH = (voltage >> 8) & 0xFF; + + telemertyPacket->sens[1].type = SENSOR_RSSI; + telemertyPacket->sens[1].number = 0; + telemertyPacket->sens[1].valueL = rssi_dBm & 0xFF; + telemertyPacket->sens[1].valueH = (rssi_dBm >> 8) & 0xFF; + + telemertyPacket->sens[2].type = SENSOR_ERR_RATE; + telemertyPacket->sens[2].number = 0; + telemertyPacket->sens[2].valueL = errorRate & 0xFF; + telemertyPacket->sens[2].valueH = (errorRate >> 8) & 0xFF; + + memset (&telemertyPacket->sens[3], 0xFF, 4 * sizeof(flySky2ASens_t)); + + 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 */ + } +} + + +static void checkRSSI (void) +{ + static uint8_t buf[RSSI_SAMPLE_COUNT] = {0}; + static int16_t sum = 0; + static uint16_t currentIndex = 0; + + uint8_t adcValue = A7105ReadReg(A7105_1D_RSSI_THOLD); + + sum += adcValue; + sum -= buf[currentIndex]; + buf[currentIndex] = adcValue; + currentIndex = (currentIndex + 1) % RSSI_SAMPLE_COUNT; + + rssi_dBm = 50 + sum / (3 * RSSI_SAMPLE_COUNT); /* range about [95...52], -dBm */ + + 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 uint8_t getNextChannel (uint8_t step) +{ + static uint8_t channel = 0; + channel = (channel + step) & 0x0F; + return rfChannelMap[channel]; +} + + +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; + } + else + { + timeout = timings->packet; + countTimeout++; + } + } +} + + +#endif /* USE_RX_FLYSKY */ diff --git a/src/main/rx/flysky.h b/src/main/rx/flysky.h new file mode 100644 index 0000000000..0c3d7df41b --- /dev/null +++ b/src/main/rx/flysky.h @@ -0,0 +1,43 @@ +/* +* 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_ + +#include +#include + + +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 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 new file mode 100644 index 0000000000..ae52109040 --- /dev/null +++ b/src/main/rx/flysky_defs.h @@ -0,0 +1,120 @@ +/* +* 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_ + +#include +#include + + +#ifndef FLYSKY_CHANNEL_COUNT +#define FLYSKY_CHANNEL_COUNT 8 +#endif + +#ifndef FLYSKY_2A_CHANNEL_COUNT +#define FLYSKY_2A_CHANNEL_COUNT 14 +#endif + + +#define FLYSKY_PAYLOAD_SIZE 21 +#define FLYSKY_2A_PAYLOAD_SIZE 37 +#define NUMFREQ 16 +#define RSSI_SAMPLE_COUNT 16 + + +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; + uint32_t txId; + uint32_t rxId; + flySky2ASens_t sens[7]; +} flySky2ATelemetryPkt_t; + + +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]; +} flySky2ABindPkt_t; + + +typedef struct __attribute__((packed)) +{ + uint8_t type; + uint32_t txId; + uint32_t rxId; + uint8_t data[28]; +} flySky2ARcDataPkt_t; + + +typedef struct __attribute__((packed)) +{ + uint8_t type; + uint32_t txId; + uint8_t data[16]; +} flySkyRcDataPkt_t; + + +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 +{ + 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/rx/rx_spi.c b/src/main/rx/rx_spi.c index a6c141b8bf..b9675c55cf 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -39,6 +39,8 @@ #include "rx/nrf24_v202.h" #include "rx/nrf24_h8_3d.h" #include "rx/nrf24_inav.h" +#include "rx/flysky.h" + uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE]; @@ -115,6 +117,14 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolDataReceived = frSkyDDataReceived; protocolSetRcDataFromPayload = frSkyDSetRcData; break; +#endif +#ifdef USE_RX_FLYSKY + case RX_SPI_A7105_FLYSKY: + case RX_SPI_A7105_FLYSKY_2A: + protocolInit = flySkyInit; + protocolDataReceived = flySkyDataReceived; + protocolSetRcDataFromPayload = flySkySetRcDataFromPayload; + break; #endif } return true; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index fd6438ef3a..1d33dd7b5e 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -30,6 +30,8 @@ typedef enum { RX_SPI_NRF24_H8_3D, RX_SPI_NRF24_INAV, RX_SPI_FRSKY_D, + RX_SPI_A7105_FLYSKY, + RX_SPI_A7105_FLYSKY_2A, RX_SPI_PROTOCOL_COUNT } rx_spi_protocol_e; diff --git a/src/main/target/EACHIF3/target.c b/src/main/target/EACHIF3/target.c new file mode 100644 index 0000000000..8609a3c79e --- /dev/null +++ b/src/main/target/EACHIF3/target.c @@ -0,0 +1,34 @@ +/* + * 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 "drivers/io.h" + +#include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, TIMER_INPUT_ENABLED), /* PPM IN */ + DEF_TIM(TIM17,CH1N,PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), /* PWM1 */ + DEF_TIM(TIM8, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), /* PWM2 */ + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), /* PWM3 */ + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), /* PWM4 */ + DEF_TIM(TIM16,CH1, PA6, TIM_USE_LED, TIMER_OUTPUT_ENABLED), /* LED_STRIP*/ +}; diff --git a/src/main/target/EACHIF3/target.h b/src/main/target/EACHIF3/target.h new file mode 100644 index 0000000000..7a8abd18b8 --- /dev/null +++ b/src/main/target/EACHIF3/target.h @@ -0,0 +1,127 @@ +/* + * 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 + +#define TARGET_BOARD_IDENTIFIER "EACH" /* https://github.com/vladisenko/EachiWhoop */ + +#define LED0_PIN PA8 + + +#define USE_EXTI +#define MPU_INT_EXTI PA15 +#define USE_MPU_DATA_READY_SIGNAL + + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + + +#define MPU6500_SPI_INSTANCE SPI1 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 +#define MPU6500_CS_PIN PA5 + + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW90_DEG + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW90_DEG + + +#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 + +#define RX_SPI_INSTANCE SPI2 +#define RX_IRQ_PIN PB12 +#define SPI2_NSS_PIN PA4 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define RX_NSS_PIN SPI2_NSS_PIN +#define RX_FLYSKY_BIND_PIN PA1 + + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) +#define I2C2_SDA PA10 +#define I2C2_SCL PA9 + + +#define BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + + +#define SERIAL_PORT_COUNT 3 +#define USE_VCP +#define USE_UART1 +#define USE_UART2 + + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_RX_PIN PA2 +#define UART2_TX_PIN PA3 + + +#define USE_ADC +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +#define VBAT_SCALE_DEFAULT 40 + + +#undef GPS +#undef MAG +#undef SONAR +#undef USE_SERVOS +#undef BEEPER + + +#define BLACKBOX +#define LED_STRIP +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define BRUSHED_MOTORS + +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP) + + +#define USB_DETECT_PIN PC14 + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define REMAP_TIM16_DMA +#define REMAP_TIM17_DMA + +#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS ( TIM_N(2) | TIM_N(8) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/EACHIF3/target.mk b/src/main/target/EACHIF3/target.mk new file mode 100644 index 0000000000..66b07fb9aa --- /dev/null +++ b/src/main/target/EACHIF3/target.mk @@ -0,0 +1,11 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/rx_a7105.c \ + rx/flysky.c From ab5c71b84f869a84fbe97bb60e1484a489157f1c Mon Sep 17 00:00:00 2001 From: "e.vladisenko" Date: Thu, 7 Sep 2017 20:16:18 +0300 Subject: [PATCH 2/3] 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 From 2b2d6e067612ee021e801394399d869e93e51672 Mon Sep 17 00:00:00 2001 From: "e.vladisenko" Date: Sun, 24 Sep 2017 10:42:38 +0300 Subject: [PATCH 3/3] Fix bind sequence --- src/main/rx/flysky.c | 56 ++++++++++++++++++++------------------- src/main/rx/flysky_defs.h | 3 +++ 2 files changed, 32 insertions(+), 27 deletions(-) diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c index d3bada92f8..60ae50a36e 100644 --- a/src/main/rx/flysky.c +++ b/src/main/rx/flysky.c @@ -102,15 +102,18 @@ static rx_spi_protocol_e protocol = RX_SPI_A7105_FLYSKY_2A; static const timings_t *timings = &flySky2ATimings; static uint32_t timeout = 0; static uint32_t timeLastPacket = 0; +static uint32_t timeLastBind = 0; +static uint32_t timeTxRequest = 0; static uint32_t countTimeout = 0; static uint32_t countPacket = 0; static uint32_t txId = 0; static uint32_t rxId = 0; static bool bound = false; static bool sendTelemetry = false; +static bool waitTx = false; static uint16_t errorRate = 0; static uint16_t rssi_dBm = 0; -static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT]; +static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT] = {0}; static uint8_t getNextChannel (uint8_t step) @@ -148,11 +151,8 @@ static void checkTimeout (void) static uint32_t timeLastTelemetry = 0; uint32_t time = micros(); - if ((time - timeMeasuareErrRate) > (100 * timings->packet)) { - if (countPacket > 100) { - countPacket = 100; - } - errorRate = 100 - countPacket; + if ((time - timeMeasuareErrRate) > (101 * timings->packet)) { + errorRate = (countPacket >= 100) ? (0) : (100 - countPacket); countPacket = 0; timeMeasuareErrRate = time; } @@ -240,7 +240,6 @@ static void buildAndWriteTelemetry (uint8_t *packet) static 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 = (bound) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + FLYSKY_FREQUENCY_COUNT); @@ -263,14 +262,15 @@ static rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_ if (sendTelemetry) { buildAndWriteTelemetry(packet); - newState = A7105_TX; sendTelemetry = false; + timeTxRequest = timeStamp; + waitTx = true; } result = RX_SPI_RECEIVED_DATA; } - if (newState != A7105_TX) { + if (!waitTx) { A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1)); } } @@ -291,15 +291,10 @@ static rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_ 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; - } + timeTxRequest = timeLastBind = timeStamp; + waitTx = true; A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE); - - newState = A7105_TX; } break; @@ -307,7 +302,9 @@ static rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_ break; } - A7105Strobe(newState); + if (!waitTx){ + A7105Strobe(A7105_RX); + } return result; } @@ -342,9 +339,7 @@ static rx_spi_received_e flySkyReadAndProcess (uint8_t *payload, const uint32_t A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(0)); - bound = true; - - result = RX_SPI_RECEIVED_BIND; + timeLastBind = timeStamp; } A7105Strobe(A7105_RX); @@ -431,15 +426,22 @@ rx_spi_received_e flySkyDataReceived (uint8_t *payload) } } - if (bound) { - checkTimeout(); + if (waitTx && (micros() - timeTxRequest) > TX_DELAY) { + A7105Strobe(A7105_TX); + waitTx = false; } - 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(); + if (bound) { + checkTimeout(); + } else { + if ((micros() - timeLastBind) > BIND_TIMEOUT && rfChannelMap[0] != 0 && txId != 0) { + result = RX_SPI_RECEIVED_BIND; + bound = true; + flySkyConfigMutable()->txId = txId; // store TXID + memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, FLYSKY_FREQUENCY_COUNT);// store channel map + flySkyConfigMutable()->protocol = protocol; + writeEEPROM(); + } } return result; diff --git a/src/main/rx/flysky_defs.h b/src/main/rx/flysky_defs.h index df2eeca0bc..71f3db1d13 100644 --- a/src/main/rx/flysky_defs.h +++ b/src/main/rx/flysky_defs.h @@ -37,6 +37,9 @@ #define FLYSKY_2A_CHANNEL_COUNT MAX_FLYSKY_2A_CHANNEL_COUNT #endif +#define TX_DELAY 500 +#define BIND_TIMEOUT 200000 + typedef struct __attribute__((packed)) { uint8_t type; uint8_t number;