diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 603cf78be1..0c4d3a2f2e 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -115,7 +115,8 @@ #define PG_CAMERA_CONTROL_CONFIG 522 #define PG_FRSKY_D_CONFIG 523 #define PG_MAX7456_CONFIG 524 -#define PG_BETAFLIGHT_END 524 +#define PG_FLYSKY_CONFIG 525 +#define PG_BETAFLIGHT_END 525 // 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..cb23008625 --- /dev/null +++ b/src/main/drivers/rx_a7105.c @@ -0,0 +1,192 @@ +/* + * 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_FLYSKY + +#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..eb9c57b800 --- /dev/null +++ b/src/main/drivers/rx_a7105.h @@ -0,0 +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 . + */ + +#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, + 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); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 0d3d6b5fa4..c88a8a7b24 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -188,7 +188,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..60ae50a36e --- /dev/null +++ b/src/main/rx/flysky.c @@ -0,0 +1,450 @@ +/* + * 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 > MAX_FLYSKY_CHANNEL_COUNT +#error "FlySky AFHDS protocol support 8 channel max" +#endif + +#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[] = { + 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 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] = {0}; + + +static uint8_t getNextChannel (uint8_t step) +{ + static uint8_t channel = 0; + channel = (channel + step) & 0x0F; + return rfChannelMap[channel]; +} + +static 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 < 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) > (101 * timings->packet)) { + errorRate = (countPacket >= 100) ? (0) : (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++; + } + } +} + +static void checkRSSI (void) +{ + static uint8_t buf[FLYSKY_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) % FLYSKY_RSSI_SAMPLE_COUNT; + + rssi_dBm = 50 + sum / (3 * FLYSKY_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 bool isValidPacket (const uint8_t *packet) { + const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet; + return (rcPacket->rxId == rxId && rcPacket->txId == txId); +} + +static 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 rx_spi_received_e flySky2AReadAndProcess (uint8_t *payload, const uint32_t timeStamp) +{ + rx_spi_received_e result = RX_SPI_RECEIVED_NONE; + uint8_t packet[FLYSKY_2A_PAYLOAD_SIZE]; + + uint8_t bytesToRead = (bound) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + FLYSKY_FREQUENCY_COUNT); + 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); + sendTelemetry = false; + timeTxRequest = timeStamp; + waitTx = true; + } + + result = RX_SPI_RECEIVED_DATA; + } + + if (!waitTx) { + A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1)); + } + } + 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 + + timeTxRequest = timeLastBind = timeStamp; + waitTx = true; + + A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE); + } + break; + + default: + break; + } + + if (!waitTx){ + A7105Strobe(A7105_RX); + } + 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)); + + timeLastBind = timeStamp; + } + + 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 (waitTx && (micros() - timeTxRequest) > TX_DELAY) { + A7105Strobe(A7105_TX); + waitTx = false; + } + + 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; +} + +#endif /* USE_RX_FLYSKY */ diff --git a/src/main/rx/flysky.h b/src/main/rx/flysky.h new file mode 100644 index 0000000000..7862f01926 --- /dev/null +++ b/src/main/rx/flysky.h @@ -0,0 +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 . + */ + +#pragma once + +#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); diff --git a/src/main/rx/flysky_defs.h b/src/main/rx/flysky_defs.h new file mode 100644 index 0000000000..71f3db1d13 --- /dev/null +++ b/src/main/rx/flysky_defs.h @@ -0,0 +1,105 @@ +/* + * 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 + +#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 MAX_FLYSKY_CHANNEL_COUNT +#endif + +#ifndef FLYSKY_2A_CHANNEL_COUNT +#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; + 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 +}; 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..79f2bbb3d3 --- /dev/null +++ b/src/main/target/EACHIF3/target.h @@ -0,0 +1,126 @@ +/* + * 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_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