diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index d1bbc2184f4..3b3e47676e5 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -111,7 +111,8 @@ #define PG_SPI_PIN_CONFIG 520 #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 -#define PG_BETAFLIGHT_END 522 +#define PG_FRSKY_D_CONFIG 523 +#define PG_BETAFLIGHT_END 523 // OSD configuration (subject to change) diff --git a/src/main/drivers/cc2500.c b/src/main/drivers/cc2500.c new file mode 100644 index 00000000000..b85b3d600c5 --- /dev/null +++ b/src/main/drivers/cc2500.c @@ -0,0 +1,85 @@ +/* +* CC2500 SPI drivers +*/ +#include +#include +#include + +#include "platform.h" + +#ifdef USE_RX_CC2500 + +#include "build/build_config.h" +#include "cc2500.h" +#include "io.h" +#include "rx_spi.h" +#include "system.h" +#include "time.h" + +#define NOP 0xFF + +uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len) +{ + return rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len); +} + +uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len) +{ + uint8_t ret; + cc2500_strobe(CC2500_SFTX); // 0x3B SFTX + ret = rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, + dpbuffer, len); + cc2500_strobe(CC2500_STX); // 0x35 + return ret; +} + +uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) +{ + return rxSpiReadCommandMulti(address, NOP, data, length); +} + +uint8_t cc2500_WriteRegisterMulti(uint8_t address, uint8_t *data, + uint8_t length) +{ + return rxSpiWriteCommandMulti(address, data, length); +} + +uint8_t cc2500_readReg(uint8_t reg) +{ + return rxSpiReadCommand(reg | 0x80, NOP); +} + +void cc2500_strobe(uint8_t address) { rxSpiWriteByte(address); } + +uint8_t cc2500_writeReg(uint8_t address, uint8_t data) +{ + return rxSpiWriteCommand(address, data); +} + +void CC2500_SetPower(uint8_t power) +{ + const uint8_t patable[8] = { + 0xC5, // -12dbm + 0x97, // -10dbm + 0x6E, // -8dbm + 0x7F, // -6dbm + 0xA9, // -4dbm + 0xBB, // -2dbm + 0xFE, // 0dbm + 0xFF // 1.5dbm + }; + if (power > 7) + power = 7; + cc2500_writeReg(CC2500_3E_PATABLE, patable[power]); +} + +uint8_t CC2500_Reset() +{ + cc2500_strobe(CC2500_SRES); + delayMicroseconds(1000); // 1000us + // CC2500_SetTxRxMode(TXRX_OFF); + // RX_EN_off;//off tx + // TX_EN_off;//off rx + return cc2500_readReg(CC2500_0E_FREQ1) == 0xC4; // check if reset +} +#endif diff --git a/src/main/drivers/cc2500.h b/src/main/drivers/cc2500.h new file mode 100644 index 00000000000..88530127b1d --- /dev/null +++ b/src/main/drivers/cc2500.h @@ -0,0 +1,153 @@ + +/* + CC2500 SPI drivers +*/ + +#pragma once + +#include +#include + +#include "rx_spi.h" + +enum { + CC2500_00_IOCFG2 = 0x00, // GDO2 output pin configuration + CC2500_01_IOCFG1 = 0x01, // GDO1 output pin configuration + CC2500_02_IOCFG0 = 0x02, // GDO0 output pin configuration + CC2500_03_FIFOTHR = 0x03, // RX FIFO and TX FIFO thresholds + CC2500_04_SYNC1 = 0x04, // Sync word, high byte + CC2500_05_SYNC0 = 0x05, // Sync word, low byte + CC2500_06_PKTLEN = 0x06, // Packet length + CC2500_07_PKTCTRL1 = 0x07, // Packet automation control + CC2500_08_PKTCTRL0 = 0x08, // Packet automation control + CC2500_09_ADDR = 0x09, // Device address + CC2500_0A_CHANNR = 0x0A, // Channel number + CC2500_0B_FSCTRL1 = 0x0B, // Frequency synthesizer control + CC2500_0C_FSCTRL0 = 0x0C, // Frequency synthesizer control + CC2500_0D_FREQ2 = 0x0D, // Frequency control word, high byte + CC2500_0E_FREQ1 = 0x0E, // Frequency control word, middle byte + CC2500_0F_FREQ0 = 0x0F, // Frequency control word, low byte + CC2500_10_MDMCFG4 = 0x10, // Modem configuration + CC2500_11_MDMCFG3 = 0x11, // Modem configuration + CC2500_12_MDMCFG2 = 0x12, // Modem configuration + CC2500_13_MDMCFG1 = 0x13, // Modem configuration + CC2500_14_MDMCFG0 = 0x14, // Modem configuration + CC2500_15_DEVIATN = 0x15, // Modem deviation setting + CC2500_16_MCSM2 = 0x16, // Main Radio Cntrl State Machine config + CC2500_17_MCSM1 = 0x17, // Main Radio Cntrl State Machine config + CC2500_18_MCSM0 = 0x18, // Main Radio Cntrl State Machine config + CC2500_19_FOCCFG = 0x19, // Frequency Offset Compensation config + CC2500_1A_BSCFG = 0x1A, // Bit Synchronization configuration + CC2500_1B_AGCCTRL2 = 0x1B, // AGC control + CC2500_1C_AGCCTRL1 = 0x1C, // AGC control + CC2500_1D_AGCCTRL0 = 0x1D, // AGC control + CC2500_1E_WOREVT1 = 0x1E, // High byte Event 0 timeout + CC2500_1F_WOREVT0 = 0x1F, // Low byte Event 0 timeout + CC2500_20_WORCTRL = 0x20, // Wake On Radio control + CC2500_21_FREND1 = 0x21, // Front end RX configuration + CC2500_22_FREND0 = 0x22, // Front end TX configuration + CC2500_23_FSCAL3 = 0x23, // Frequency synthesizer calibration + CC2500_24_FSCAL2 = 0x24, // Frequency synthesizer calibration + CC2500_25_FSCAL1 = 0x25, // Frequency synthesizer calibration + CC2500_26_FSCAL0 = 0x26, // Frequency synthesizer calibration + CC2500_27_RCCTRL1 = 0x27, // RC oscillator configuration + CC2500_28_RCCTRL0 = 0x28, // RC oscillator configuration + CC2500_29_FSTEST = 0x29, // Frequency synthesizer cal control + CC2500_2A_PTEST = 0x2A, // Production test + CC2500_2B_AGCTEST = 0x2B, // AGC test + CC2500_2C_TEST2 = 0x2C, // Various test settings + CC2500_2D_TEST1 = 0x2D, // Various test settings + CC2500_2E_TEST0 = 0x2E, // Various test settings + + // Status registers + CC2500_30_PARTNUM = 0x30, // Part number + CC2500_31_VERSION = 0x31, // Current version number + CC2500_32_FREQEST = 0x32, // Frequency offset estimate + CC2500_33_LQI = 0x33, // Demodulator estimate for link quality + CC2500_34_RSSI = 0x34, // Received signal strength indication + CC2500_35_MARCSTATE = 0x35, // Control state machine state + CC2500_36_WORTIME1 = 0x36, // High byte of WOR timer + CC2500_37_WORTIME0 = 0x37, // Low byte of WOR timer + CC2500_38_PKTSTATUS = 0x38, // Current GDOx status and packet status + CC2500_39_VCO_VC_DAC = 0x39, // Current setting from PLL cal module + CC2500_3A_TXBYTES = 0x3A, // Underflow and # of bytes in TXFIFO + CC2500_3B_RXBYTES = 0x3B, // Overflow and # of bytes in RXFIFO + + // Multi byte memory locations + CC2500_3E_PATABLE = 0x3E, + CC2500_3F_TXFIFO = 0x3F, + CC2500_3F_RXFIFO = 0x3F +}; + +// Definitions for burst/single access to registers +#define CC2500_WRITE_SINGLE 0x00 +#define CC2500_WRITE_BURST 0x40 +#define CC2500_READ_SINGLE 0x80 +#define CC2500_READ_BURST 0xC0 + +// Strobe commands +#define CC2500_SRES 0x30 // Reset chip. +#define CC2500_SFSTXON \ + 0x31 // Enable and calibrate frequency synthesizer (if MCSM0.FS_AUTOCAL=1). + // If in RX/TX: Go to a wait state where only the synthesizer is + // running (for quick RX / TX turnaround). +#define CC2500_SXOFF 0x32 // Turn off crystal oscillator. +#define CC2500_SCAL 0x33 // Calibrate frequency synthesizer and turn it off + // (enables quick start). +#define CC2500_SRX \ + 0x34 // Enable RX. Perform calibration first if coming from IDLE and + // MCSM0.FS_AUTOCAL=1. +#define CC2500_STX \ + 0x35 // In IDLE state: Enable TX. Perform calibration first if + // MCSM0.FS_AUTOCAL=1. If in RX state and CCA is enabled: + // Only go to TX if channel is clear. +#define CC2500_SIDLE \ + 0x36 // Exit RX / TX, turn off frequency synthesizer and exit + // Wake-On-Radio mode if applicable. +#define CC2500_SAFC 0x37 // Perform AFC adjustment of the frequency synthesizer +#define CC2500_SWOR 0x38 // Start automatic RX polling sequence (Wake-on-Radio) +#define CC2500_SPWD 0x39 // Enter power down mode when CSn goes high. +#define CC2500_SFRX 0x3A // Flush the RX FIFO buffer. +#define CC2500_SFTX 0x3B // Flush the TX FIFO buffer. +#define CC2500_SWORRST 0x3C // Reset real time clock. +#define CC2500_SNOP \ + 0x3D // No operation. May be used to pad strobe commands to two + // bytes for simpler software. +//---------------------------------------------------------------------------------- +// Chip Status Byte +//---------------------------------------------------------------------------------- + +// Bit fields in the chip status byte +#define CC2500_STATUS_CHIP_RDYn_BM 0x80 +#define CC2500_STATUS_STATE_BM 0x70 +#define CC2500_STATUS_FIFO_BYTES_AVAILABLE_BM 0x0F + +// Chip states +#define CC2500_STATE_IDLE 0x00 +#define CC2500_STATE_RX 0x10 +#define CC2500_STATE_TX 0x20 +#define CC2500_STATE_FSTXON 0x30 +#define CC2500_STATE_CALIBRATE 0x40 +#define CC2500_STATE_SETTLING 0x50 +#define CC2500_STATE_RX_OVERFLOW 0x60 +#define CC2500_STATE_TX_UNDERFLOW 0x70 + +//---------------------------------------------------------------------------------- +// Other register bit fields +//---------------------------------------------------------------------------------- +#define CC2500_LQI_CRC_OK_BM 0x80 +#define CC2500_LQI_EST_BM 0x7F + +// void FrskyRXspiInit(); +uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len); +uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len); +uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data, + uint8_t length); +uint8_t CC2500_WriteRegisterMulti(uint8_t address, uint8_t *data, + uint8_t length); + +uint8_t cc2500_readReg(uint8_t reg); +void cc2500_strobe(uint8_t address); +uint8_t cc2500_writeReg(uint8_t address, uint8_t data); +void CC2500_SetPower(uint8_t power); +uint8_t CC2500_Reset(); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1245bdd6e9b..cd5f95594ad 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -116,6 +116,7 @@ extern uint8_t __config_end; #include "rx/rx.h" #include "rx/spektrum.h" +#include "rx/frsky_d.h" #include "scheduler/scheduler.h" @@ -2109,6 +2110,13 @@ static void cliBeeper(char *cmdline) } #endif +#ifdef FRSKY_BIND +void cliFrSkyBind(char *cmdline){ + UNUSED(cmdline); + frSkyDBind(); +} +#endif + static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { bool equalsDefault = true; @@ -3483,6 +3491,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif +#ifdef FRSKY_BIND + CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), +#endif #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 43828888876..5797edb8f3b 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -63,6 +63,7 @@ #include "rx/rx.h" #include "rx/spektrum.h" +#include "rx/frsky_d.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -727,7 +728,14 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_ESC_SENSOR - { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) }, + { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) }, +#endif + +#ifdef USE_RX_FRSKYD + { "frsky_d_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, autoBind) }, + { "frsky_d_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindTxId) }, + { "frsky_d_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindOffset) }, + { "frsky_d_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindHopData) }, #endif { "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, #ifdef USE_DASHBOARD diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c new file mode 100644 index 00000000000..6567ad43ed5 --- /dev/null +++ b/src/main/rx/frsky_d.c @@ -0,0 +1,744 @@ +/* + * 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 + +#include "platform.h" + +#ifdef USE_RX_FRSKYD + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "drivers/cc2500.h" +#include "drivers/io.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" +#include "rx/frsky_d.h" + +#include "sensors/battery.h" + +#include "fc/config.h" + +#include "config/feature.h" +#include "config/parameter_group_ids.h" + +#include "telemetry/frsky.h" + +#define RC_CHANNEL_COUNT 8 +#define MAX_MISSING_PKT 100 + +#define SYNC 9000 +#define FS_THR 960 + +#define FLED_on \ + { \ + IOHi(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \ + } +#define FLED_off \ + { \ + IOLo(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \ + } +#define GDO_1 IORead(IOGetByTag(IO_TAG(GDO_0_PIN))) + +#if defined(PA_LNA) +#define TX_EN_on \ + { \ + IOHi(IOGetByTag(IO_TAG(TX_EN_PIN))); \ + } +#define TX_EN_off \ + { \ + IOLo(IOGetByTag(IO_TAG(TX_EN_PIN))); \ + } + +#define RX_EN_on \ + { \ + IOHi(IOGetByTag(IO_TAG(RX_EN_PIN))); \ + } +#define RX_EN_off \ + { \ + IOLo(IOGetByTag(IO_TAG(RX_EN_PIN))); \ + } + +#if defined(DIVERSITY) +#define ANT_SEL_on \ + { \ + IOHi(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \ + } +#define ANT_SEL_off \ + { \ + IOLo(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \ + } +#endif +#endif + +static uint8_t ccLen; +static uint8_t channr; +static uint32_t missingPackets; +static uint8_t calData[255][3]; +static uint32_t time_tune; +static bool eol; +static uint8_t listLength; +static uint8_t bindIdx; +static uint8_t cnt; +static uint16_t Servo_data[RC_CHANNEL_COUNT]; +static uint16_t c[8]; +static uint32_t t_out; +static uint8_t Lqi; +static uint32_t packet_timer; +static uint8_t protocol_state; +static int16_t word_temp; +static uint32_t start_time; + +static IO_t GdoPin; +static IO_t BindPin = DEFIO_IO(NONE); +static IO_t FrskyLedPin; +#if defined(PA_LNA) +static IO_t TxEnPin; +static IO_t RxEnPin; +static IO_t AntSelPin; +static uint8_t pass; +#endif +bool bindRequested = false; + +#ifdef FRSKY_TELEMETRY +static uint8_t frame[20]; +static int16_t RSSI_dBm; +static uint8_t telemetry_id; +static uint8_t telemetryRX; +static uint8_t v1; // A1 +static uint8_t v2; // A2 +static uint32_t time_t; + +#if defined(HUB) +#define MAX_SERIAL_BYTES 64 +uint8_t hub_index; +uint8_t idxx = 0; +uint8_t idx_ok = 0; +uint8_t telemetry_expected_id = 0; +uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data +#endif +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, PG_FRSKY_D_CONFIG, + 0); + +PG_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, .autoBind = false, + .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .bindTxId = {0, 0}, .bindOffset = 0); + +rx_spi_received_e ret; + +enum { + STATE_INIT = 0, + STATE_BIND, + STATE_BIND_TUNING, + STATE_BIND_BINDING1, + STATE_BIND_BINDING2, + STATE_BIND_COMPLETE, + STATE_STARTING, + STATE_UPDATE, + STATE_DATA, + STATE_TELEM +}; + +#if defined(FRSKY_TELEMETRY) +static void compute_RSSIdbm(uint8_t *packet) +{ + if (packet[ccLen - 2] >= 128) { + RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) - 82; + } else { + RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) + 65; + } +} + +#if defined(HUB) +static uint8_t frsky_append_hub_data(uint8_t *buf) +{ + if (telemetry_id == telemetry_expected_id) + idx_ok = idxx; + else // rx re-requests last packet + idxx = idx_ok; + + telemetry_expected_id = (telemetry_id + 1) & 0x1F; + uint8_t index = 0; + for (uint8_t i = 0; i < 10; i++) { + if (idxx == hub_index) { + break; + } + buf[i] = srx_data[idxx]; + idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1); + index++; + } + return index; +} + +static void frSkyTelemetryInitFrameSpi(void) { hub_index = 0; } + +static void frSkyTelemetryWriteSpi(uint8_t ch) { srx_data[hub_index++] = ch; } +#endif + +static void telemetry_build_frame(uint8_t *packet) +{ +#ifdef USE_ADC + // if(feature(FEATURE_VBAT)){ + // v1 =vbat; + // v2=0; + // } + // else + v1 = 0; +#endif + uint8_t bytes_used = 0; + telemetry_id = packet[4]; + frame[0] = 0x11; // length + frame[1] = frSkyDConfig()->bindTxId[0]; + frame[2] = frSkyDConfig()->bindTxId[1]; + frame[3] = v1; // A1 voltages + frame[4] = v2; // A2 + frame[5] = (uint8_t)RSSI_dBm; +#if defined(HUB) + bytes_used = frsky_append_hub_data(&frame[8]); +#endif + frame[6] = bytes_used; + frame[7] = telemetry_id; +} + +#endif + +#if defined(PA_LNA) +static void RX_enable() +{ + TX_EN_off; + RX_EN_on; +} + +static void TX_enable() +{ + RX_EN_off; + TX_EN_on; +} +#endif + +void frSkyDBind() { bindRequested = true; } + +static void initialize() +{ + CC2500_Reset(); + cc2500_writeReg(CC2500_02_IOCFG0, 0x01); + cc2500_writeReg(CC2500_17_MCSM1, 0x0C); + cc2500_writeReg(CC2500_18_MCSM0, 0x18); + cc2500_writeReg(CC2500_06_PKTLEN, 0x19); + cc2500_writeReg(CC2500_08_PKTCTRL0, 0x05); + cc2500_writeReg(CC2500_3E_PATABLE, 0xFF); + cc2500_writeReg(CC2500_0B_FSCTRL1, 0x08); + cc2500_writeReg(CC2500_0C_FSCTRL0, 0x00); + cc2500_writeReg(CC2500_0D_FREQ2, 0x5C); + cc2500_writeReg(CC2500_0E_FREQ1, 0x76); + cc2500_writeReg(CC2500_0F_FREQ0, 0x27); + cc2500_writeReg(CC2500_10_MDMCFG4, 0xAA); + cc2500_writeReg(CC2500_11_MDMCFG3, 0x39); + cc2500_writeReg(CC2500_12_MDMCFG2, 0x11); + cc2500_writeReg(CC2500_13_MDMCFG1, 0x23); + cc2500_writeReg(CC2500_14_MDMCFG0, 0x7A); + cc2500_writeReg(CC2500_15_DEVIATN, 0x42); + cc2500_writeReg(CC2500_19_FOCCFG, 0x16); + cc2500_writeReg(CC2500_1A_BSCFG, 0x6C); + cc2500_writeReg(CC2500_1B_AGCCTRL2, 0x03); + cc2500_writeReg(CC2500_1C_AGCCTRL1, 0x40); + cc2500_writeReg(CC2500_1D_AGCCTRL0, 0x91); + cc2500_writeReg(CC2500_21_FREND1, 0x56); + cc2500_writeReg(CC2500_22_FREND0, 0x10); + cc2500_writeReg(CC2500_23_FSCAL3, 0xA9); + cc2500_writeReg(CC2500_24_FSCAL2, 0x0A); + cc2500_writeReg(CC2500_25_FSCAL1, 0x00); + cc2500_writeReg(CC2500_26_FSCAL0, 0x11); + cc2500_writeReg(CC2500_29_FSTEST, 0x59); + cc2500_writeReg(CC2500_2C_TEST2, 0x88); + cc2500_writeReg(CC2500_2D_TEST1, 0x31); + cc2500_writeReg(CC2500_2E_TEST0, 0x0B); + cc2500_writeReg(CC2500_03_FIFOTHR, 0x07); + cc2500_writeReg(CC2500_09_ADDR, 0x00); + cc2500_strobe(CC2500_SIDLE); + + cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04); + cc2500_writeReg(CC2500_0C_FSCTRL0, 0); + for (uint8_t c = 0; c < 0xFF; c++) { + cc2500_strobe(CC2500_SIDLE); + cc2500_writeReg(CC2500_0A_CHANNR, c); + cc2500_strobe(CC2500_SCAL); + delayMicroseconds(900); + calData[c][0] = cc2500_readReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500_readReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500_readReg(CC2500_25_FSCAL1); + } +} + +static void initialize_data(uint8_t adr) +{ + cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); + cc2500_writeReg(CC2500_18_MCSM0, 0x8); + cc2500_writeReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]); + cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0D); + cc2500_writeReg(CC2500_19_FOCCFG, 0x16); + delay(10); +} + +static void initTuneRx(void) +{ + cc2500_writeReg(CC2500_19_FOCCFG, 0x14); + time_tune = millis(); + frSkyDConfigMutable()->bindOffset = -126; + cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); + cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0C); + cc2500_writeReg(CC2500_18_MCSM0, 0x8); + + cc2500_strobe(CC2500_SIDLE); + cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500_writeReg(CC2500_0A_CHANNR, 0); + cc2500_strobe(CC2500_SFRX); + cc2500_strobe(CC2500_SRX); +} + +static bool tuneRx(uint8_t *packet) +{ + if (frSkyDConfig()->bindOffset >= 126) { + frSkyDConfigMutable()->bindOffset = -126; + } + if ((millis() - time_tune) > 50) { + time_tune = millis(); + frSkyDConfigMutable()->bindOffset += 5; + cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); + } + if (GDO_1) { + ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500_readFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + Lqi = packet[ccLen - 1] & 0x7F; + if (Lqi < 50) { + return true; + } + } + } + } + } + + return false; +} + +static void initGetBind(void) +{ + cc2500_strobe(CC2500_SIDLE); + cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500_writeReg(CC2500_0A_CHANNR, 0); + cc2500_strobe(CC2500_SFRX); + delayMicroseconds(20); // waiting flush FIFO + + cc2500_strobe(CC2500_SRX); + eol = false; + listLength = 0; + bindIdx = 0x05; +} + +static bool getBind1(uint8_t *packet) +{ + // len|bind |tx + // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC| + // Start by getting bind packet 0 and the txid + if (GDO_1) { + ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500_readFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + if (packet[5] == 0x00) { + frSkyDConfigMutable()->bindTxId[0] = packet[3]; + frSkyDConfigMutable()->bindTxId[1] = packet[4]; + for (uint8_t n = 0; n < 5; n++) { + frSkyDConfigMutable()->bindHopData[packet[5] + n] = + packet[6 + n]; + } + return true; + } + } + } + } + } + + return false; +} + +static bool getBind2(uint8_t *packet) +{ + if (bindIdx <= 120) { + if (GDO_1) { + ccLen = + cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500_readFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + if ((packet[3] == frSkyDConfig()->bindTxId[0]) && + (packet[4] == frSkyDConfig()->bindTxId[1])) { + if (packet[5] == bindIdx) { +#if defined(DJTS) + if (packet[5] == 0x2D) { + for (uint8_t i = 0; i < 2; i++) { + frSkyDConfigMutable() + ->bindHopData[packet[5] + i] = + packet[6 + i]; + } + listLength = 47; + eol = true; + + bindIdx = bindIdx + 5; + return false; + } +#endif + for (uint8_t n = 0; n < 5; n++) { + if (packet[6 + n] == packet[ccLen - 3] || + (packet[6 + n] == 0)) { + if (bindIdx >= 0x2D) { + eol = true; + listLength = packet[5] + n; + break; + } + } + frSkyDConfigMutable() + ->bindHopData[packet[5] + n] = + packet[6 + n]; + } + + bindIdx = bindIdx + 5; + return false; + } + } + } + } + } + } + + if (eol) { + return true; + } + } + + return false; +} + +static void nextChannel(uint8_t skip) +{ + channr += skip; + if (channr >= listLength) + channr -= listLength; + cc2500_strobe(CC2500_SIDLE); + cc2500_writeReg(CC2500_23_FSCAL3, + calData[frSkyDConfig()->bindHopData[channr]][0]); + cc2500_writeReg(CC2500_24_FSCAL2, + calData[frSkyDConfig()->bindHopData[channr]][1]); + cc2500_writeReg(CC2500_25_FSCAL1, + calData[frSkyDConfig()->bindHopData[channr]][2]); + cc2500_writeReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]); + cc2500_strobe(CC2500_SFRX); +} + +static bool bindButtonPressed(void) +{ + if (!IORead(BindPin)) { + delayMicroseconds(10); + return true; + } + + return false; +} + +static bool doBind() +{ + return frSkyDConfig()->autoBind || bindRequested || bindButtonPressed(); +} + +void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *packet) +{ + c[0] = (uint16_t)(((packet[10] & 0x0F) << 8 | packet[6])); + c[1] = (uint16_t)(((packet[10] & 0xF0) << 4 | packet[7])); + c[2] = (uint16_t)(((packet[11] & 0x0F) << 8 | packet[8])); + c[3] = (uint16_t)(((packet[11] & 0xF0) << 4 | packet[9])); + c[4] = (uint16_t)(((packet[16] & 0x0F) << 8 | packet[12])); + c[5] = (uint16_t)(((packet[16] & 0xF0) << 4 | packet[13])); + c[6] = (uint16_t)(((packet[17] & 0x0F) << 8 | packet[14])); + c[7] = (uint16_t)(((packet[17] & 0xF0) << 4 | packet[15])); + + for (uint8_t i = 0; i < 8; i++) { + word_temp = 0.667 * c[i]; + if ((word_temp > 800) && (word_temp < 2200)) + Servo_data[i] = word_temp; + rcData[i] = Servo_data[i]; + } +} + +rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) +{ + ret = RX_SPI_RECEIVED_NONE; + + switch (protocol_state) { + case STATE_INIT: + if ((millis() - start_time) > 10) { + initialize(); + + protocol_state = STATE_BIND; + } + + break; + case STATE_BIND: + if (doBind()) { + bindRequested = false; + + FLED_on; + initTuneRx(); + + protocol_state = STATE_BIND_TUNING; + } else { + protocol_state = STATE_STARTING; + } + + break; + case STATE_BIND_TUNING: + if (tuneRx(packet)) { + initGetBind(); + initialize_data(1); + + protocol_state = STATE_BIND_BINDING1; + } + + break; + case STATE_BIND_BINDING1: + if (getBind1(packet)) { + protocol_state = STATE_BIND_BINDING2; + } + + break; + case STATE_BIND_BINDING2: + if (getBind2(packet)) { + cc2500_strobe(CC2500_SIDLE); + + protocol_state = STATE_BIND_COMPLETE; + } + + break; + case STATE_BIND_COMPLETE: + if (!frSkyDConfig()->autoBind) { + writeEEPROM(); + } else { + uint8_t ctr = 40; + while (ctr--) { + FLED_on; + delay(50); + FLED_off; + delay(50); + } + } + + protocol_state = STATE_STARTING; + + break; + case STATE_STARTING: + listLength = 47; + initialize_data(0); + protocol_state = STATE_UPDATE; + nextChannel(1); // + cc2500_strobe(CC2500_SRX); + ret = RX_SPI_RECEIVED_BIND; + + break; + case STATE_UPDATE: + packet_timer = micros(); + protocol_state = STATE_DATA; + if (bindRequested) { + packet_timer = 0; + t_out = 50; + missingPackets = 0; + protocol_state = STATE_INIT; + break; + } + // here FS code could be + case STATE_DATA: + if (GDO_1) { + ccLen = + cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen > 20) + ccLen = 20; + if (ccLen == 20) { + cc2500_readFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + missingPackets = 0; + t_out = 1; + if (packet[0] == 0x11) { + if ((packet[1] == frSkyDConfig()->bindTxId[0]) && + (packet[2] == frSkyDConfig()->bindTxId[1])) { + FLED_on; + nextChannel(1); +#ifdef FRSKY_TELEMETRY + if ((packet[3] % 4) == 2) { + telemetryRX = 1; + time_t = micros(); + compute_RSSIdbm(packet); + telemetry_build_frame(packet); + protocol_state = STATE_TELEM; + } else +#endif + { + cc2500_strobe(CC2500_SRX); + protocol_state = STATE_UPDATE; + } + ret = RX_SPI_RECEIVED_DATA; + packet_timer = micros(); + } + } + } + } + } + if ((micros() - packet_timer) > (t_out * SYNC)) { +#ifdef PA_LNA + RX_enable(); +#endif + if (t_out == 1) { +#ifdef DIVERSITY // SE4311 chip + if (missingPackets >= 2) { + if (pass & 0x01) { + ANT_SEL_on; + } else { + ANT_SEL_off; + } + pass++; + } +#endif + + if (missingPackets > MAX_MISSING_PKT) + t_out = 50; + + missingPackets++; + nextChannel(1); + } else { + if (cnt++ & 0x01) { + FLED_off; + } else + FLED_on; + + nextChannel(13); + } + + cc2500_strobe(CC2500_SRX); + protocol_state = STATE_UPDATE; + } + break; +#ifdef FRSKY_TELEMETRY + case STATE_TELEM: + if (telemetryRX) { + if ((micros() - time_t) >= 1380) { + cc2500_strobe(CC2500_SIDLE); + CC2500_SetPower(6); + cc2500_strobe(CC2500_SFRX); +#if defined(PA_LNA) + TX_enable(); +#endif + cc2500_strobe(CC2500_SIDLE); + cc2500_writeFifo(frame, frame[0] + 1); + protocol_state = STATE_DATA; + ret = RX_SPI_RECEIVED_DATA; + packet_timer = micros(); + telemetryRX = 0; + } + } + + break; + +#endif + } + return ret; +} + +void frskyD_Rx_Setup(rx_spi_protocol_e protocol) +{ + UNUSED(protocol); + // gpio init here + GdoPin = IOGetByTag(IO_TAG(GDO_0_PIN)); + BindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); + FrskyLedPin = IOGetByTag(IO_TAG(FRSKY_LED_PIN)); +#if defined(PA_LNA) + RxEnPin = IOGetByTag(IO_TAG(RX_EN_PIN)); + TxEnPin = IOGetByTag(IO_TAG(TX_EN_PIN)); + IOInit(RxEnPin, OWNER_RX_BIND, 0); + IOInit(TxEnPin, OWNER_RX_BIND, 0); + IOConfigGPIO(RxEnPin, IOCFG_OUT_PP); + IOConfigGPIO(TxEnPin, IOCFG_OUT_PP); +#endif +#if defined(DIVERSITY) + AntSelPin = IOGetByTag(IO_TAG(ANT_SEL_PIN)); + IOInit(AntSelPin, OWNER_RX_BIND, 0); + IOConfigGPIO(AntSelPin, IOCFG_OUT_PP); +#endif + // + IOInit(GdoPin, OWNER_RX_BIND, 0); + IOInit(BindPin, OWNER_RX_BIND, 0); + IOInit(FrskyLedPin, OWNER_LED, 0); + // + IOConfigGPIO(GdoPin, IOCFG_IN_FLOATING); + IOConfigGPIO(BindPin, IOCFG_IPU); + IOConfigGPIO(FrskyLedPin, IOCFG_OUT_PP); + start_time = millis(); + packet_timer = 0; + t_out = 50; + missingPackets = 0; + protocol_state = STATE_INIT; +#if defined(DIVERSITY) + ANT_SEL_on; +#endif +#if defined(PA_LNA) + RX_enable(); +#endif + +#if defined(HUB) + initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, + &frSkyTelemetryWriteSpi); +#endif + + // if(!frskySpiDetect())//detect spi working routine + // return; +} + +void frskyD_Rx_Init(const rxConfig_t *rxConfig, + rxRuntimeConfig_t *rxRuntimeConfig) +{ + rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; + frskyD_Rx_Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol); +} +#endif diff --git a/src/main/rx/frsky_d.h b/src/main/rx/frsky_d.h new file mode 100644 index 00000000000..427fb517373 --- /dev/null +++ b/src/main/rx/frsky_d.h @@ -0,0 +1,39 @@ +/* + * 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 + +#include "rx_spi.h" + +typedef struct frSkyDConfig_s { + bool autoBind; + uint8_t bindHopData[50]; + uint8_t bindTxId[2]; + int8_t bindOffset; +} frSkyDConfig_t; + +PG_DECLARE(frSkyDConfig_t, frSkyDConfig); + +struct rxConfig_s; +struct rxRuntimeConfig_s; +void frskyD_Rx_Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *payload); +rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *payload); +void frSkyDBind(); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index ac4764f1506..089107fa634 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -24,6 +24,7 @@ #include "build/build_config.h" +#include "drivers/cc2500.h" #include "drivers/rx_nrf24l01.h" #include "config/feature.h" @@ -32,13 +33,13 @@ #include "rx/rx.h" #include "rx/rx_spi.h" +#include "rx/frsky_d.h" #include "rx/nrf24_cx10.h" #include "rx/nrf24_syma.h" #include "rx/nrf24_v202.h" #include "rx/nrf24_h8_3d.h" #include "rx/nrf24_inav.h" - uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE]; STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received @@ -53,7 +54,9 @@ static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload; STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { + BUILD_BUG_ON(NRF24L01_MAX_PAYLOAD_SIZE > RX_SPI_MAX_PAYLOAD_SIZE); + if (channel >= rxRuntimeConfig->channelCount) { return 0; } @@ -105,6 +108,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolDataReceived = inavNrf24DataReceived; protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload; break; +#endif +#ifdef USE_RX_FRSKYD + case RX_SPI_FRSKYD: + protocolInit = frskyD_Rx_Init; + protocolDataReceived = frskyD_Rx_DataReceived; + protocolSetRcDataFromPayload = frskyD_Rx_SetRCdata; + break; #endif } return true; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index c858279564a..1946dea285f 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -29,6 +29,7 @@ typedef enum { RX_SPI_NRF24_CX10A, RX_SPI_NRF24_H8_3D, RX_SPI_NRF24_INAV, + RX_SPI_FRSKYD, RX_SPI_PROTOCOL_COUNT } rx_spi_protocol_e; diff --git a/src/main/target/MIDELICF3/MIDELICF3V2.mk b/src/main/target/MIDELICF3/MIDELICF3V2.mk new file mode 100644 index 00000000000..3a673c1ea97 --- /dev/null +++ b/src/main/target/MIDELICF3/MIDELICF3V2.mk @@ -0,0 +1 @@ +#added V2 target in order to free USB_DM and USB_DP pins, connect USB directly and remove USB-Serial chip from main board \ No newline at end of file diff --git a/src/main/target/MIDELICF3/target.c b/src/main/target/MIDELICF3/target.c new file mode 100644 index 00000000000..471d124a837 --- /dev/null +++ b/src/main/target/MIDELICF3/target.c @@ -0,0 +1,45 @@ +/* + * 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/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { +#if defined(MIDELICF3V2) + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1), // PWM1 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM2 - PA3 + DEF_TIM(TIM4, CH3, PA13, TIM_USE_MOTOR, 1), // PWM3 - PA12 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM4 - PA11 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_MOTOR, 1), // PWM5 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // PWM6 - PB9 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_LED, 1), // GPIO_TIMER / LED_STRIP +#else + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1), // PWM1 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM2 - PA3 + DEF_TIM(TIM4, CH2, PA12, TIM_USE_MOTOR, 1), // PWM3 - PA12 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MOTOR, 1), // PWM4 - PA11 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // PWM5 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // PWM6 - PB9 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_LED, 1), // GPIO_TIMER / LED_STRIP +#endif +}; diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h new file mode 100644 index 00000000000..41d0be637ca --- /dev/null +++ b/src/main/target/MIDELICF3/target.h @@ -0,0 +1,143 @@ +/* + * 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 "MIF3" + +#ifdef MIDELICF3_PROTOTYPE +//V1 prototype board +#define LED0_PIN PB3 +#else +//V1 production board +#define LED0_PIN PA13 +#endif + +#define BEEPER PC14 + +#define GYRO +#define USE_GYRO_MPU6050 +#define GYRO_MPU6050_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_MPU6050 +#define ACC_MPU6050_ALIGN CW270_DEG + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 + +#define SERIAL_PORT_COUNT 2 + +#define SPEKTRUM_BIND +#define BIND_PIN PA14 +#define BINDPLUG_PIN PC13 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_INSTANCE SPI2 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN SPI2_NSS_PIN + +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PA1 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_RX_CC2500 + +#if defined(USE_RX_CC2500) + +#define USE_RX_SPI +#define RX_SPI_INSTANCE SPI1 +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA + +#define USE_RX_FRSKYD +#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKYD +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI + +#define FRSKY_BIND + +#define FRSKY_TELEMETRY +#define HUB + +// Amplifier chip +#define PA_LNA +// Antenna switch +#define DIVERSITY +//#define SWAMPING + +//Pinout +#define RX_NSS_PIN SPI1_NSS_PIN +#define RX_SCK_PIN SPI1_SCK_PIN +#define RX_MISO_PIN SPI1_MISO_PIN +#define RX_MOSI_PIN SPI1_MOSI_PIN +#define GDO_0_PIN PB0 +#define ANT_SEL_PIN PB11 +#define TX_EN_PIN PB1 +#define RX_EN_PIN PB2 +#if defined(MIDELICF3V2) +#define FRSKY_LED_PIN PB4 +#else +#define FRSKY_LED_PIN PA8 +#endif + +#else +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#endif //USE_RX_CC2500 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 + +#define DEFAULT_FEATURES (FEATURE_AIRMODE | FEATURE_TELEMETRY) + +#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(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 7 +#if defined(MIDELICF3V2) +#define USED_TIMERS (TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16)) +#else +#define USED_TIMERS (TIM_N(3) | TIM_N(4) | TIM_N(15)) +#endif diff --git a/src/main/target/MIDELICF3/target.mk b/src/main/target/MIDELICF3/target.mk new file mode 100644 index 00000000000..a6137255173 --- /dev/null +++ b/src/main/target/MIDELICF3/target.mk @@ -0,0 +1,10 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6050.c \ + blackbox/blackbox.c \ + blackbox/blackbox_io.c \ + drivers/cc2500.c \ + rx/frsky_d.c diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 39451a247a7..07cbf9c9022 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -74,9 +74,10 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; +static frSkyTelemetryWriteFn *frSkyTelemetryWrite = NULL; +static frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrame = NULL; #define CYCLETIME 125 @@ -129,28 +130,30 @@ static portSharing_e frskyPortSharing; static uint32_t lastCycleTime = 0; static uint8_t cycleNum = 0; + static void sendDataHead(uint8_t id) { - serialWrite(frskyPort, PROTOCOL_HEADER); - serialWrite(frskyPort, id); + frSkyTelemetryWrite(PROTOCOL_HEADER); + frSkyTelemetryWrite(id); } static void sendTelemetryTail(void) { - serialWrite(frskyPort, PROTOCOL_TAIL); + frSkyTelemetryWrite(PROTOCOL_TAIL); } static void serializeFrsky(uint8_t data) { // take care of byte stuffing if (data == 0x5e) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3e); + frSkyTelemetryWrite(0x5d); + frSkyTelemetryWrite(0x3e); } else if (data == 0x5d) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3d); - } else - serialWrite(frskyPort, data); + frSkyTelemetryWrite(0x5d); + frSkyTelemetryWrite(0x3d); + } else{ + frSkyTelemetryWrite(data); + } } static void serialize16(int16_t a) @@ -456,20 +459,38 @@ static void sendHeading(void) serialize16(0); } +void frSkyTelemetryWriteSerial(uint8_t ch) +{ + serialWrite(frskyPort, ch); +} + + void initFrSkyTelemetry(void) { portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } +void initFrSkyExternalTelemetry(frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrameExternal, frSkyTelemetryWriteFn *frSkyTelemetryWriteExternal) +{ + frSkyTelemetryInitFrame = frSkyTelemetryInitFrameExternal; + frSkyTelemetryWrite = frSkyTelemetryWriteExternal; +} + +void deinitFrSkyExternalTelemetry(void) +{ + frSkyTelemetryInitFrame = NULL; + frSkyTelemetryWrite = NULL; +} + void freeFrSkyTelemetryPort(void) { closeSerialPort(frskyPort); frskyPort = NULL; - frskyTelemetryEnabled = false; + frSkyTelemetryWrite = NULL; } -void configureFrSkyTelemetryPort(void) +static void configureFrSkyTelemetryPort(void) { if (!portConfig) { return; @@ -480,7 +501,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyTelemetryEnabled = true; + frSkyTelemetryWrite = frSkyTelemetryWriteSerial; } bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) @@ -488,17 +509,22 @@ bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) return currentMillis - lastCycleTime >= CYCLETIME; } +bool checkFrSkySerialTelemetryEnabled(void) +{ + return frSkyTelemetryWrite == &frSkyTelemetryWriteSerial; +} + void checkFrSkyTelemetryState(void) { if (portConfig && telemetryCheckRxPortShared(portConfig)) { - if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) { + if (!checkFrSkySerialTelemetryEnabled() && telemetrySharedPort != NULL) { frskyPort = telemetrySharedPort; - frskyTelemetryEnabled = true; + frSkyTelemetryWrite = &frSkyTelemetryWriteSerial; } } else { bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); - if (newTelemetryEnabledValue == frskyTelemetryEnabled) { + if (newTelemetryEnabledValue == checkFrSkySerialTelemetryEnabled()) { return; } @@ -511,7 +537,7 @@ void checkFrSkyTelemetryState(void) void handleFrSkyTelemetry(void) { - if (!frskyTelemetryEnabled) { + if (frSkyTelemetryWrite == NULL) { return; } @@ -525,6 +551,10 @@ void handleFrSkyTelemetry(void) cycleNum++; + if (frSkyTelemetryInitFrame) { + frSkyTelemetryInitFrame(); + } + // Sent every 125ms sendAccel(); sendVario(); diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 255dae90dee..6e3cce6dde8 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -22,10 +22,13 @@ typedef enum { FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; +typedef void frSkyTelemetryInitFrameFn(void); +typedef void frSkyTelemetryWriteFn(uint8_t ch); + void handleFrSkyTelemetry(void); void checkFrSkyTelemetryState(void); void initFrSkyTelemetry(void); -void configureFrSkyTelemetryPort(void); -void freeFrSkyTelemetryPort(void); +void initFrSkyExternalTelemetry(frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrameExternal, frSkyTelemetryWriteFn *frSkyTelemetryWriteExternal); +void deinitFrSkyExternalTelemetry(void);