mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +03:00
Merge pull request #4683 from mikeller/added_frsky_x_spi_rx
Added FrSky X SPI RX protocol.
This commit is contained in:
Michael Keller
GitHub
commit
a72a46c604
22 changed files with 1708 additions and 848 deletions
|
|
@ -50,7 +50,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
||||||
"FFT",
|
"FFT",
|
||||||
"FFT_TIME",
|
"FFT_TIME",
|
||||||
"FFT_FREQ",
|
"FFT_FREQ",
|
||||||
"FRSKY_D_RX",
|
"RX_FRSKY_SPI",
|
||||||
"GYRO_RAW",
|
"GYRO_RAW",
|
||||||
"MAX7456_SIGNAL",
|
"MAX7456_SIGNAL",
|
||||||
"MAX7456_SPICLOCK",
|
"MAX7456_SPICLOCK",
|
||||||
|
|
|
||||||
|
|
@ -68,7 +68,7 @@ typedef enum {
|
||||||
DEBUG_FFT,
|
DEBUG_FFT,
|
||||||
DEBUG_FFT_TIME,
|
DEBUG_FFT_TIME,
|
||||||
DEBUG_FFT_FREQ,
|
DEBUG_FFT_FREQ,
|
||||||
DEBUG_FRSKY_D_RX,
|
DEBUG_RX_FRSKY_SPI,
|
||||||
DEBUG_GYRO_RAW,
|
DEBUG_GYRO_RAW,
|
||||||
DEBUG_MAX7456_SIGNAL,
|
DEBUG_MAX7456_SIGNAL,
|
||||||
DEBUG_MAX7456_SPICLOCK,
|
DEBUG_MAX7456_SPICLOCK,
|
||||||
|
|
|
||||||
|
|
@ -113,7 +113,7 @@
|
||||||
#define PG_SPI_PIN_CONFIG 520
|
#define PG_SPI_PIN_CONFIG 520
|
||||||
#define PG_ESCSERIAL_CONFIG 521
|
#define PG_ESCSERIAL_CONFIG 521
|
||||||
#define PG_CAMERA_CONTROL_CONFIG 522
|
#define PG_CAMERA_CONTROL_CONFIG 522
|
||||||
#define PG_FRSKY_D_CONFIG 523
|
#define PG_RX_FRSKY_SPI_CONFIG 523
|
||||||
#define PG_MAX7456_CONFIG 524
|
#define PG_MAX7456_CONFIG 524
|
||||||
#define PG_FLYSKY_CONFIG 525
|
#define PG_FLYSKY_CONFIG 525
|
||||||
#define PG_TIME_CONFIG 526
|
#define PG_TIME_CONFIG 526
|
||||||
|
|
|
||||||
|
|
@ -118,7 +118,8 @@ extern uint8_t __config_end;
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/spektrum.h"
|
#include "rx/spektrum.h"
|
||||||
#include "rx/frsky_d.h"
|
#include "../rx/cc2500_frsky_common.h"
|
||||||
|
#include "../rx/cc2500_frsky_x.h"
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
|
|
@ -2112,12 +2113,24 @@ static void cliBeeper(char *cmdline)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_RX_FRSKY_D
|
|
||||||
void cliFrSkyBind(char *cmdline){
|
void cliFrSkyBind(char *cmdline){
|
||||||
UNUSED(cmdline);
|
UNUSED(cmdline);
|
||||||
frSkyDBind();
|
switch (rxConfig()->rx_spi_protocol) {
|
||||||
}
|
#ifdef USE_RX_FRSKY_SPI
|
||||||
|
case RX_SPI_FRSKY_D:
|
||||||
|
case RX_SPI_FRSKY_X:
|
||||||
|
frSkyBind();
|
||||||
|
|
||||||
|
cliPrint("Binding...");
|
||||||
|
|
||||||
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
default:
|
||||||
|
cliPrint("Not supported.");
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
|
static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
|
||||||
{
|
{
|
||||||
|
|
@ -3641,8 +3654,8 @@ const clicmd_t cmdTable[] = {
|
||||||
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
|
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_RX_FRSKY_D
|
#ifdef USE_RX_FRSKY_SPI
|
||||||
CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky RX", NULL, cliFrSkyBind),
|
CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky SPI RX", NULL, cliFrSkyBind),
|
||||||
#endif
|
#endif
|
||||||
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
|
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
|
||||||
#ifdef USE_GPS
|
#ifdef USE_GPS
|
||||||
|
|
|
||||||
|
|
@ -65,8 +65,8 @@
|
||||||
#include "io/vtx_rtc6705.h"
|
#include "io/vtx_rtc6705.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
#include "rx/cc2500_frsky_common.h"
|
||||||
#include "rx/spektrum.h"
|
#include "rx/spektrum.h"
|
||||||
#include "rx/frsky_d.h"
|
|
||||||
|
|
||||||
#include "sensors/acceleration.h"
|
#include "sensors/acceleration.h"
|
||||||
#include "sensors/barometer.h"
|
#include "sensors/barometer.h"
|
||||||
|
|
@ -195,6 +195,7 @@ static const char * const lookupTableRxSpi[] = {
|
||||||
"H8_3D",
|
"H8_3D",
|
||||||
"INAV",
|
"INAV",
|
||||||
"FRSKY_D",
|
"FRSKY_D",
|
||||||
|
"FRSKY_X",
|
||||||
"FLYSKY",
|
"FLYSKY",
|
||||||
"FLYSKY_2A"
|
"FLYSKY_2A"
|
||||||
};
|
};
|
||||||
|
|
@ -658,13 +659,13 @@ const clivalue_t valueTable[] = {
|
||||||
{ "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_unit) },
|
{ "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_unit) },
|
||||||
#endif
|
#endif
|
||||||
{ "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_vfas_precision) },
|
{ "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_vfas_precision) },
|
||||||
#endif
|
#endif // USE_TELEMETRY_FRSKY
|
||||||
{ "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) },
|
{ "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) },
|
||||||
{ "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) },
|
{ "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) },
|
||||||
#if defined(USE_TELEMETRY_IBUS)
|
#if defined(USE_TELEMETRY_IBUS)
|
||||||
{ "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) },
|
{ "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) },
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif // USE_TELEMETRY
|
||||||
|
|
||||||
// PG_LED_STRIP_CONFIG
|
// PG_LED_STRIP_CONFIG
|
||||||
#ifdef USE_LED_STRIP
|
#ifdef USE_LED_STRIP
|
||||||
|
|
@ -804,11 +805,12 @@ const clivalue_t valueTable[] = {
|
||||||
{ "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
|
#endif
|
||||||
|
|
||||||
#ifdef USE_RX_FRSKY_D
|
#ifdef USE_RX_FRSKY_SPI
|
||||||
{ "frsky_d_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, autoBind) },
|
{ "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_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_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindTxId) },
|
||||||
{ "frsky_d_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindOffset) },
|
{ "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_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) },
|
{ "frsky_spi_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindHopData) },
|
||||||
|
{ "frsky_x_rx_num", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, rxNum) },
|
||||||
#endif
|
#endif
|
||||||
{ "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) },
|
{ "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
|
#ifdef USE_DASHBOARD
|
||||||
|
|
|
||||||
36
src/main/rx/cc2500_frsky_common.h
Normal file
36
src/main/rx/cc2500_frsky_common.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
//#include <stdbool.h>
|
||||||
|
//#include <stdint.h>
|
||||||
|
|
||||||
|
//#include "rx.h"
|
||||||
|
//#include "rx_spi.h"
|
||||||
|
|
||||||
|
typedef struct rxFrSkySpiConfig_s {
|
||||||
|
bool autoBind;
|
||||||
|
uint8_t bindTxId[2];
|
||||||
|
int8_t bindOffset;
|
||||||
|
uint8_t bindHopData[50];
|
||||||
|
uint8_t rxNum;
|
||||||
|
} rxFrSkySpiConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig);
|
||||||
|
|
||||||
|
void frSkyBind(void);
|
||||||
396
src/main/rx/cc2500_frsky_d.c
Normal file
396
src/main/rx/cc2500_frsky_d.c
Normal file
|
|
@ -0,0 +1,396 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_D
|
||||||
|
|
||||||
|
#include "build/build_config.h"
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/cc2500.h"
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
#include "fc/config.h"
|
||||||
|
|
||||||
|
#include "config/feature.h"
|
||||||
|
#include "config/parameter_group_ids.h"
|
||||||
|
|
||||||
|
#include "rx/rx.h"
|
||||||
|
#include "rx/rx_spi.h"
|
||||||
|
#include "rx/cc2500_frsky_common.h"
|
||||||
|
#include "rx/cc2500_frsky_shared.h"
|
||||||
|
#include "rx/cc2500_frsky_d.h"
|
||||||
|
|
||||||
|
#include "sensors/battery.h"
|
||||||
|
|
||||||
|
#include "telemetry/frsky.h"
|
||||||
|
|
||||||
|
#define RC_CHANNEL_COUNT 8
|
||||||
|
#define MAX_MISSING_PKT 100
|
||||||
|
|
||||||
|
#define DEBUG_DATA_ERROR_COUNT 0
|
||||||
|
|
||||||
|
#define SYNC 9000
|
||||||
|
#define FS_THR 960
|
||||||
|
|
||||||
|
static uint32_t missingPackets;
|
||||||
|
static uint8_t calData[255][3];
|
||||||
|
static uint8_t cnt;
|
||||||
|
static int32_t t_out;
|
||||||
|
static timeUs_t lastPacketReceivedTime;
|
||||||
|
static uint8_t protocolState;
|
||||||
|
static uint32_t start_time;
|
||||||
|
static uint16_t dataErrorCount = 0;
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
static uint8_t pass;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
static uint8_t frame[20];
|
||||||
|
static uint8_t telemetry_id;
|
||||||
|
static uint32_t telemetryTime;
|
||||||
|
|
||||||
|
#if defined(USE_TELEMETRY_FRSKY)
|
||||||
|
#define MAX_SERIAL_BYTES 64
|
||||||
|
static uint8_t hub_index;
|
||||||
|
static uint8_t idxx = 0;
|
||||||
|
static uint8_t idx_ok = 0;
|
||||||
|
static uint8_t telemetry_expected_id = 0;
|
||||||
|
static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
#if defined(USE_TELEMETRY_FRSKY)
|
||||||
|
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;
|
||||||
|
idxx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void frSkyTelemetryWriteSpi(uint8_t ch)
|
||||||
|
{
|
||||||
|
if (hub_index < MAX_SERIAL_BYTES) {
|
||||||
|
srx_data[hub_index++] = ch;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static void telemetry_build_frame(uint8_t *packet)
|
||||||
|
{
|
||||||
|
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
|
||||||
|
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
|
||||||
|
uint8_t bytes_used = 0;
|
||||||
|
telemetry_id = packet[4];
|
||||||
|
frame[0] = 0x11; // length
|
||||||
|
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
|
||||||
|
frame[2] = rxFrSkySpiConfig()->bindTxId[1];
|
||||||
|
frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
|
||||||
|
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
|
||||||
|
frame[5] = (uint8_t)RSSI_dBm;
|
||||||
|
#if defined(USE_TELEMETRY_FRSKY)
|
||||||
|
bytes_used = frsky_append_hub_data(&frame[8]);
|
||||||
|
#endif
|
||||||
|
frame[6] = bytes_used;
|
||||||
|
frame[7] = telemetry_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
|
||||||
|
static void initialize(void)
|
||||||
|
{
|
||||||
|
cc2500Reset();
|
||||||
|
cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
|
||||||
|
cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
|
||||||
|
cc2500WriteReg(CC2500_18_MCSM0, 0x18);
|
||||||
|
cc2500WriteReg(CC2500_06_PKTLEN, 0x19);
|
||||||
|
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
|
||||||
|
cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
|
||||||
|
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
|
||||||
|
cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
|
||||||
|
cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
|
||||||
|
cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
|
||||||
|
cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA);
|
||||||
|
cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
|
||||||
|
cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
|
||||||
|
cc2500WriteReg(CC2500_13_MDMCFG1, 0x23);
|
||||||
|
cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A);
|
||||||
|
cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
|
||||||
|
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
|
||||||
|
cc2500WriteReg(CC2500_1A_BSCFG, 0x6C);
|
||||||
|
cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
|
||||||
|
cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
|
||||||
|
cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
|
||||||
|
cc2500WriteReg(CC2500_21_FREND1, 0x56);
|
||||||
|
cc2500WriteReg(CC2500_22_FREND0, 0x10);
|
||||||
|
cc2500WriteReg(CC2500_23_FSCAL3, 0xA9);
|
||||||
|
cc2500WriteReg(CC2500_24_FSCAL2, 0x0A);
|
||||||
|
cc2500WriteReg(CC2500_25_FSCAL1, 0x00);
|
||||||
|
cc2500WriteReg(CC2500_26_FSCAL0, 0x11);
|
||||||
|
cc2500WriteReg(CC2500_29_FSTEST, 0x59);
|
||||||
|
cc2500WriteReg(CC2500_2C_TEST2, 0x88);
|
||||||
|
cc2500WriteReg(CC2500_2D_TEST1, 0x31);
|
||||||
|
cc2500WriteReg(CC2500_2E_TEST0, 0x0B);
|
||||||
|
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
|
||||||
|
cc2500WriteReg(CC2500_09_ADDR, 0x00);
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
|
||||||
|
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04);
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, 0);
|
||||||
|
for (uint8_t c = 0; c < 0xFF; c++) {
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteReg(CC2500_0A_CHANNR, c);
|
||||||
|
cc2500Strobe(CC2500_SCAL);
|
||||||
|
delayMicroseconds(900);
|
||||||
|
calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
|
||||||
|
calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
|
||||||
|
calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
|
||||||
|
|
||||||
|
static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
|
||||||
|
channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
|
||||||
|
channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
|
||||||
|
{
|
||||||
|
uint16_t channels[RC_CHANNEL_COUNT];
|
||||||
|
bool dataError = false;
|
||||||
|
|
||||||
|
decodeChannelPair(channels, packet + 6, 4);
|
||||||
|
decodeChannelPair(channels + 2, packet + 8, 3);
|
||||||
|
decodeChannelPair(channels + 4, packet + 12, 4);
|
||||||
|
decodeChannelPair(channels + 6, packet + 14, 3);
|
||||||
|
|
||||||
|
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
|
||||||
|
if ((channels[i] < 800) || (channels[i] > 2200)) {
|
||||||
|
dataError = true;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!dataError) {
|
||||||
|
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
|
||||||
|
rcData[i] = channels[i];
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
|
||||||
|
{
|
||||||
|
const timeUs_t currentPacketReceivedTime = micros();
|
||||||
|
|
||||||
|
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
|
||||||
|
|
||||||
|
switch (protocolState) {
|
||||||
|
case STATE_INIT:
|
||||||
|
if ((millis() - start_time) > 10) {
|
||||||
|
initialize();
|
||||||
|
|
||||||
|
protocolState = STATE_BIND;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND:
|
||||||
|
case STATE_BIND_TUNING:
|
||||||
|
case STATE_BIND_BINDING1:
|
||||||
|
case STATE_BIND_BINDING2:
|
||||||
|
case STATE_BIND_COMPLETE:
|
||||||
|
handleBinding(protocolState, packet);
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_STARTING:
|
||||||
|
listLength = 47;
|
||||||
|
initialiseData(0);
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
nextChannel(1, true); //
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
ret = RX_SPI_RECEIVED_BIND;
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_UPDATE:
|
||||||
|
lastPacketReceivedTime = currentPacketReceivedTime;
|
||||||
|
protocolState = STATE_DATA;
|
||||||
|
|
||||||
|
if (checkBindRequested(false)) {
|
||||||
|
lastPacketReceivedTime = 0;
|
||||||
|
t_out = 50;
|
||||||
|
missingPackets = 0;
|
||||||
|
|
||||||
|
protocolState = STATE_INIT;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// here FS code could be
|
||||||
|
case STATE_DATA:
|
||||||
|
if (IORead(gdoPin)) {
|
||||||
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
if (ccLen >= 20) {
|
||||||
|
cc2500ReadFifo(packet, 20);
|
||||||
|
if (packet[19] & 0x80) {
|
||||||
|
missingPackets = 0;
|
||||||
|
t_out = 1;
|
||||||
|
if (packet[0] == 0x11) {
|
||||||
|
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
|
||||||
|
(packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
nextChannel(1, true);
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
if ((packet[3] % 4) == 2) {
|
||||||
|
telemetryTime = micros();
|
||||||
|
setRssiDbm(packet[18]);
|
||||||
|
telemetry_build_frame(packet);
|
||||||
|
protocolState = STATE_TELEMETRY;
|
||||||
|
} else
|
||||||
|
#endif
|
||||||
|
{
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
}
|
||||||
|
ret = RX_SPI_RECEIVED_DATA;
|
||||||
|
lastPacketReceivedTime = currentPacketReceivedTime;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) {
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
RxEnable();
|
||||||
|
#endif
|
||||||
|
if (t_out == 1) {
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip
|
||||||
|
if (missingPackets >= 2) {
|
||||||
|
if (pass & 0x01) {
|
||||||
|
IOHi(antSelPin);
|
||||||
|
} else {
|
||||||
|
IOLo(antSelPin);
|
||||||
|
}
|
||||||
|
pass++;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (missingPackets > MAX_MISSING_PKT) {
|
||||||
|
t_out = 50;
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
missingPackets++;
|
||||||
|
nextChannel(1, true);
|
||||||
|
} else {
|
||||||
|
if (cnt++ & 0x01) {
|
||||||
|
IOLo(frSkyLedPin);
|
||||||
|
} else {
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL);
|
||||||
|
#endif
|
||||||
|
nextChannel(13, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
case STATE_TELEMETRY:
|
||||||
|
if ((micros() - telemetryTime) >= 1380) {
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500SetPower(6);
|
||||||
|
cc2500Strobe(CC2500_SFRX);
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
TxEnable();
|
||||||
|
#endif
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteFifo(frame, frame[0] + 1);
|
||||||
|
protocolState = STATE_DATA;
|
||||||
|
ret = RX_SPI_RECEIVED_DATA;
|
||||||
|
lastPacketReceivedTime = currentPacketReceivedTime;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void frSkyDInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
|
{
|
||||||
|
UNUSED(rxConfig);
|
||||||
|
|
||||||
|
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
|
||||||
|
|
||||||
|
frskySpiRxSetup();
|
||||||
|
|
||||||
|
lastPacketReceivedTime = 0;
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY)
|
||||||
|
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
|
||||||
|
&frSkyTelemetryWriteSpi);
|
||||||
|
#endif
|
||||||
|
if (rssiSource == RSSI_SOURCE_NONE) {
|
||||||
|
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -17,23 +17,10 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include "rx.h"
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include "rx_spi.h"
|
#include "rx_spi.h"
|
||||||
|
|
||||||
typedef struct frSkyDConfig_s {
|
|
||||||
bool autoBind;
|
|
||||||
uint8_t bindTxId[2];
|
|
||||||
int8_t bindOffset;
|
|
||||||
uint8_t bindHopData[50];
|
|
||||||
} frSkyDConfig_t;
|
|
||||||
|
|
||||||
PG_DECLARE(frSkyDConfig_t, frSkyDConfig);
|
|
||||||
|
|
||||||
struct rxConfig_s;
|
|
||||||
struct rxRuntimeConfig_s;
|
|
||||||
void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
||||||
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload);
|
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e frSkyDDataReceived(uint8_t *payload);
|
rx_spi_received_e frSkyDDataReceived(uint8_t *payload);
|
||||||
void frSkyDBind(void);
|
void frSkyBind(void);
|
||||||
408
src/main/rx/cc2500_frsky_shared.c
Normal file
408
src/main/rx/cc2500_frsky_shared.c
Normal file
|
|
@ -0,0 +1,408 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_RX_FRSKY_SPI
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
#include "drivers/cc2500.h"
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
#include "fc/config.h"
|
||||||
|
|
||||||
|
#include "config/parameter_group.h"
|
||||||
|
#include "config/parameter_group_ids.h"
|
||||||
|
|
||||||
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
#include "rx/cc2500_frsky_common.h"
|
||||||
|
|
||||||
|
#include "cc2500_frsky_shared.h"
|
||||||
|
|
||||||
|
static uint32_t missingPackets;
|
||||||
|
static uint8_t calData[255][3];
|
||||||
|
static timeMs_t timeTunedMs;
|
||||||
|
uint8_t listLength;
|
||||||
|
static uint8_t bindIdx;
|
||||||
|
static uint8_t Lqi;
|
||||||
|
static uint8_t protocolState;
|
||||||
|
static timeMs_t timeStartedMs;
|
||||||
|
static int8_t bindOffset;
|
||||||
|
static bool lastBindPinStatus;
|
||||||
|
bool bindRequested = false;
|
||||||
|
|
||||||
|
IO_t gdoPin;
|
||||||
|
static IO_t bindPin = DEFIO_IO(NONE);
|
||||||
|
IO_t frSkyLedPin;
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
static IO_t txEnPin;
|
||||||
|
static IO_t rxLnaEnPin;
|
||||||
|
IO_t antSelPin;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
int16_t RSSI_dBm;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 0);
|
||||||
|
|
||||||
|
PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig,
|
||||||
|
.autoBind = false,
|
||||||
|
.bindTxId = {0, 0},
|
||||||
|
.bindOffset = 0,
|
||||||
|
.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},
|
||||||
|
.rxNum = 0,
|
||||||
|
);
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
void setRssiDbm(uint8_t value)
|
||||||
|
{
|
||||||
|
if (value >= 128) {
|
||||||
|
RSSI_dBm = ((((uint16_t)value) * 18) >> 5) - 82;
|
||||||
|
} else {
|
||||||
|
RSSI_dBm = ((((uint16_t)value) * 18) >> 5) + 65;
|
||||||
|
}
|
||||||
|
|
||||||
|
setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1023), RSSI_SOURCE_RX_PROTOCOL);
|
||||||
|
}
|
||||||
|
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
void RxEnable(void)
|
||||||
|
{
|
||||||
|
IOLo(txEnPin);
|
||||||
|
}
|
||||||
|
|
||||||
|
void TxEnable(void)
|
||||||
|
{
|
||||||
|
IOHi(txEnPin);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void frSkyBind(void)
|
||||||
|
{
|
||||||
|
bindRequested = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void initialiseData(uint8_t adr)
|
||||||
|
{
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxFrSkySpiConfig()->bindOffset);
|
||||||
|
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
|
||||||
|
cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxFrSkySpiConfig()->bindTxId[0]);
|
||||||
|
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
|
||||||
|
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
|
||||||
|
delay(10);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void initTuneRx(void)
|
||||||
|
{
|
||||||
|
cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
|
||||||
|
timeTunedMs = millis();
|
||||||
|
bindOffset = -126;
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
||||||
|
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
|
||||||
|
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
|
||||||
|
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
|
||||||
|
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
|
||||||
|
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
|
||||||
|
cc2500WriteReg(CC2500_0A_CHANNR, 0);
|
||||||
|
cc2500Strobe(CC2500_SFRX);
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool tuneRx(uint8_t *packet)
|
||||||
|
{
|
||||||
|
if (bindOffset >= 126) {
|
||||||
|
bindOffset = -126;
|
||||||
|
}
|
||||||
|
if ((millis() - timeTunedMs) > 50) {
|
||||||
|
timeTunedMs = millis();
|
||||||
|
bindOffset += 5;
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
||||||
|
}
|
||||||
|
if (IORead(gdoPin)) {
|
||||||
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
if (ccLen) {
|
||||||
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
if (packet[ccLen - 1] & 0x80) {
|
||||||
|
if (packet[2] == 0x01) {
|
||||||
|
Lqi = packet[ccLen - 1] & 0x7F;
|
||||||
|
if (Lqi < 50) {
|
||||||
|
rxFrSkySpiConfigMutable()->bindOffset = bindOffset;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void initGetBind(void)
|
||||||
|
{
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
|
||||||
|
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
|
||||||
|
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
|
||||||
|
cc2500WriteReg(CC2500_0A_CHANNR, 0);
|
||||||
|
cc2500Strobe(CC2500_SFRX);
|
||||||
|
delayMicroseconds(20); // waiting flush FIFO
|
||||||
|
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
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 (IORead(gdoPin)) {
|
||||||
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
if (ccLen) {
|
||||||
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
if (packet[ccLen - 1] & 0x80) {
|
||||||
|
if (packet[2] == 0x01) {
|
||||||
|
if (packet[5] == 0x00) {
|
||||||
|
rxFrSkySpiConfigMutable()->bindTxId[0] = packet[3];
|
||||||
|
rxFrSkySpiConfigMutable()->bindTxId[1] = packet[4];
|
||||||
|
for (uint8_t n = 0; n < 5; n++) {
|
||||||
|
rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] =
|
||||||
|
packet[6 + n];
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool getBind2(uint8_t *packet)
|
||||||
|
{
|
||||||
|
if (bindIdx <= 120) {
|
||||||
|
if (IORead(gdoPin)) {
|
||||||
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
if (ccLen) {
|
||||||
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
if (packet[ccLen - 1] & 0x80) {
|
||||||
|
if (packet[2] == 0x01) {
|
||||||
|
if ((packet[3] == rxFrSkySpiConfig()->bindTxId[0]) &&
|
||||||
|
(packet[4] == rxFrSkySpiConfig()->bindTxId[1])) {
|
||||||
|
if (packet[5] == bindIdx) {
|
||||||
|
#if defined(DJTS)
|
||||||
|
if (packet[5] == 0x2D) {
|
||||||
|
for (uint8_t i = 0; i < 2; i++) {
|
||||||
|
rxFrSkySpiConfigMutable()->bindHopData[packet[5] + i] = packet[6 + i];
|
||||||
|
}
|
||||||
|
listLength = 47;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
for (uint8_t n = 0; n < 5; n++) {
|
||||||
|
if (packet[6 + n] == packet[ccLen - 3] || (packet[6 + n] == 0)) {
|
||||||
|
if (bindIdx >= 0x2D) {
|
||||||
|
listLength = packet[5] + n;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n];
|
||||||
|
}
|
||||||
|
|
||||||
|
bindIdx = bindIdx + 5;
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
} else {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool checkBindRequested(bool reset)
|
||||||
|
{
|
||||||
|
if (bindPin) {
|
||||||
|
bool bindPinStatus = IORead(bindPin);
|
||||||
|
if (lastBindPinStatus && !bindPinStatus) {
|
||||||
|
bindRequested = true;
|
||||||
|
}
|
||||||
|
lastBindPinStatus = bindPinStatus;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!bindRequested) {
|
||||||
|
return false;
|
||||||
|
} else {
|
||||||
|
if (reset) {
|
||||||
|
bindRequested = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleBinding(uint8_t protocolState, uint8_t *packet)
|
||||||
|
{
|
||||||
|
switch (protocolState) {
|
||||||
|
case STATE_BIND:
|
||||||
|
if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) {
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
initTuneRx();
|
||||||
|
|
||||||
|
protocolState = STATE_BIND_TUNING;
|
||||||
|
} else {
|
||||||
|
protocolState = STATE_STARTING;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND_TUNING:
|
||||||
|
if (tuneRx(packet)) {
|
||||||
|
initGetBind();
|
||||||
|
initialiseData(1);
|
||||||
|
|
||||||
|
protocolState = STATE_BIND_BINDING1;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND_BINDING1:
|
||||||
|
if (getBind1(packet)) {
|
||||||
|
protocolState = STATE_BIND_BINDING2;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND_BINDING2:
|
||||||
|
if (getBind2(packet)) {
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
|
||||||
|
protocolState = STATE_BIND_COMPLETE;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND_COMPLETE:
|
||||||
|
if (!rxFrSkySpiConfig()->autoBind) {
|
||||||
|
writeEEPROM();
|
||||||
|
} else {
|
||||||
|
uint8_t ctr = 40;
|
||||||
|
while (ctr--) {
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
delay(50);
|
||||||
|
IOLo(frSkyLedPin);
|
||||||
|
delay(50);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
protocolState = STATE_STARTING;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void nextChannel(uint8_t skip, bool sendStrobe)
|
||||||
|
{
|
||||||
|
static uint8_t channr = 0;
|
||||||
|
|
||||||
|
channr += skip;
|
||||||
|
while (channr >= listLength) {
|
||||||
|
channr -= listLength;
|
||||||
|
}
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteReg(CC2500_23_FSCAL3,
|
||||||
|
calData[rxFrSkySpiConfig()->bindHopData[channr]][0]);
|
||||||
|
cc2500WriteReg(CC2500_24_FSCAL2,
|
||||||
|
calData[rxFrSkySpiConfig()->bindHopData[channr]][1]);
|
||||||
|
cc2500WriteReg(CC2500_25_FSCAL1,
|
||||||
|
calData[rxFrSkySpiConfig()->bindHopData[channr]][2]);
|
||||||
|
cc2500WriteReg(CC2500_0A_CHANNR, rxFrSkySpiConfig()->bindHopData[channr]);
|
||||||
|
if (sendStrobe) {
|
||||||
|
cc2500Strobe(CC2500_SFRX);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void frskySpiRxSetup()
|
||||||
|
{
|
||||||
|
// gpio init here
|
||||||
|
gdoPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_GDO_0_PIN));
|
||||||
|
IOInit(gdoPin, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
|
||||||
|
frSkyLedPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LED_PIN));
|
||||||
|
IOInit(frSkyLedPin, OWNER_LED, 0);
|
||||||
|
IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP);
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
rxLnaEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LNA_EN_PIN));
|
||||||
|
IOInit(rxLnaEnPin, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP);
|
||||||
|
IOHi(rxLnaEnPin); // always on at the moment
|
||||||
|
txEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_TX_EN_PIN));
|
||||||
|
IOInit(txEnPin, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(txEnPin, IOCFG_OUT_PP);
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_DIVERSITY)
|
||||||
|
antSelPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_ANT_SEL_PIN));
|
||||||
|
IOInit(antSelPin, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
|
||||||
|
#endif
|
||||||
|
#endif // USE_RX_FRSKY_SPI_PA_LNA
|
||||||
|
#if defined(BINDPLUG_PIN)
|
||||||
|
bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
|
||||||
|
IOInit(bindPin, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(bindPin, IOCFG_IPU);
|
||||||
|
|
||||||
|
lastBindPinStatus = IORead(bindPin);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
timeStartedMs = millis();
|
||||||
|
missingPackets = 0;
|
||||||
|
protocolState = STATE_INIT;
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_DIVERSITY)
|
||||||
|
IOHi(antSelPin);
|
||||||
|
#endif
|
||||||
|
RxEnable();
|
||||||
|
#endif // USE_RX_FRSKY_SPI_PA_LNA
|
||||||
|
|
||||||
|
if (rssiSource == RSSI_SOURCE_NONE) {
|
||||||
|
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// if(!frSkySpiDetect())//detect spi working routine
|
||||||
|
// return;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
61
src/main/rx/cc2500_frsky_shared.h
Normal file
61
src/main/rx/cc2500_frsky_shared.h
Normal file
|
|
@ -0,0 +1,61 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#define MAX_MISSING_PKT 100
|
||||||
|
|
||||||
|
#define DEBUG_DATA_ERROR_COUNT 0
|
||||||
|
|
||||||
|
#define SYNC 9000
|
||||||
|
|
||||||
|
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_TELEMETRY,
|
||||||
|
STATE_RESUME,
|
||||||
|
};
|
||||||
|
|
||||||
|
extern bool bindRequested;
|
||||||
|
extern uint8_t listLength;
|
||||||
|
extern int16_t RSSI_dBm;
|
||||||
|
|
||||||
|
extern IO_t gdoPin;
|
||||||
|
extern IO_t frSkyLedPin;
|
||||||
|
extern IO_t antSelPin;
|
||||||
|
|
||||||
|
void setRssiDbm(uint8_t value);
|
||||||
|
|
||||||
|
void frskySpiRxSetup();
|
||||||
|
|
||||||
|
void RxEnable(void);
|
||||||
|
void TxEnable(void);
|
||||||
|
|
||||||
|
void initialiseData(uint8_t adr);
|
||||||
|
|
||||||
|
bool checkBindRequested(bool reset);
|
||||||
|
|
||||||
|
void handleBinding(uint8_t protocolState, uint8_t *packet);
|
||||||
|
|
||||||
|
void nextChannel(uint8_t skip, bool sendStrobe);
|
||||||
630
src/main/rx/cc2500_frsky_x.c
Normal file
630
src/main/rx/cc2500_frsky_x.c
Normal file
|
|
@ -0,0 +1,630 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#include <sys/_stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_X
|
||||||
|
|
||||||
|
#include "build/build_config.h"
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#include "config/feature.h"
|
||||||
|
#include "config/parameter_group.h"
|
||||||
|
#include "config/parameter_group_ids.h"
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/adc.h"
|
||||||
|
#include "drivers/cc2500.h"
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/io_def.h"
|
||||||
|
#include "drivers/io_types.h"
|
||||||
|
#include "drivers/resource.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
#include "fc/config.h"
|
||||||
|
|
||||||
|
#include "rx/rx.h"
|
||||||
|
#include "rx/rx_spi.h"
|
||||||
|
#include "rx/cc2500_frsky_common.h"
|
||||||
|
#include "rx/cc2500_frsky_shared.h"
|
||||||
|
#include "rx/cc2500_frsky_x.h"
|
||||||
|
|
||||||
|
#include "sensors/battery.h"
|
||||||
|
|
||||||
|
#include "telemetry/smartport.h"
|
||||||
|
|
||||||
|
#define RC_CHANNEL_COUNT 16
|
||||||
|
|
||||||
|
const uint16_t CRCTable[] = {
|
||||||
|
0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
|
||||||
|
0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
|
||||||
|
0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
|
||||||
|
0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876,
|
||||||
|
0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd,
|
||||||
|
0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5,
|
||||||
|
0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c,
|
||||||
|
0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974,
|
||||||
|
0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb,
|
||||||
|
0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3,
|
||||||
|
0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a,
|
||||||
|
0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72,
|
||||||
|
0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9,
|
||||||
|
0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1,
|
||||||
|
0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738,
|
||||||
|
0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70,
|
||||||
|
0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7,
|
||||||
|
0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff,
|
||||||
|
0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036,
|
||||||
|
0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e,
|
||||||
|
0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5,
|
||||||
|
0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd,
|
||||||
|
0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134,
|
||||||
|
0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c,
|
||||||
|
0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3,
|
||||||
|
0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb,
|
||||||
|
0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232,
|
||||||
|
0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a,
|
||||||
|
0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1,
|
||||||
|
0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9,
|
||||||
|
0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330,
|
||||||
|
0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78
|
||||||
|
};
|
||||||
|
|
||||||
|
#define TELEMETRY_OUT_BUFFER_SIZE 64
|
||||||
|
|
||||||
|
#define TELEMETRY_SEQUENCE_LENGTH 4
|
||||||
|
|
||||||
|
typedef struct telemetrySequenceMarkerData_s {
|
||||||
|
unsigned int packetSequenceId: 2;
|
||||||
|
unsigned int unused: 1;
|
||||||
|
unsigned int initRequest: 1;
|
||||||
|
unsigned int ackSequenceId: 2;
|
||||||
|
unsigned int retransmissionRequested: 1;
|
||||||
|
unsigned int initResponse: 1;
|
||||||
|
} __attribute__ ((__packed__)) telemetrySequenceMarkerData_t;
|
||||||
|
|
||||||
|
typedef union telemetrySequenceMarker_s {
|
||||||
|
uint8_t raw;
|
||||||
|
telemetrySequenceMarkerData_t data;
|
||||||
|
} __attribute__ ((__packed__)) telemetrySequenceMarker_t;
|
||||||
|
|
||||||
|
#define SEQUENCE_MARKER_REMOTE_PART 0xf0
|
||||||
|
|
||||||
|
#define TELEMETRY_DATA_SIZE 5
|
||||||
|
|
||||||
|
typedef struct telemetryData_s {
|
||||||
|
uint8_t dataLength;
|
||||||
|
uint8_t data[TELEMETRY_DATA_SIZE];
|
||||||
|
} __attribute__ ((__packed__)) telemetryData_t;
|
||||||
|
|
||||||
|
typedef struct telemetryBuffer_s {
|
||||||
|
telemetryData_t data;
|
||||||
|
uint8_t needsProcessing;
|
||||||
|
} telemetryBuffer_t;
|
||||||
|
|
||||||
|
#define TELEMETRY_FRAME_SIZE sizeof(telemetryData_t)
|
||||||
|
|
||||||
|
typedef struct telemetryPayload_s {
|
||||||
|
uint8_t packetConst;
|
||||||
|
uint8_t rssiA1;
|
||||||
|
telemetrySequenceMarker_t sequence;
|
||||||
|
telemetryData_t data;
|
||||||
|
uint8_t crc[2];
|
||||||
|
} __attribute__ ((__packed__)) telemetryPayload_t;
|
||||||
|
|
||||||
|
static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
|
||||||
|
static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH];
|
||||||
|
|
||||||
|
static uint8_t remoteProcessedId = 0;
|
||||||
|
static uint8_t remoteAckId = 0;
|
||||||
|
|
||||||
|
static uint8_t remoteToProcessIndex = 0;
|
||||||
|
|
||||||
|
static uint8_t localPacketId;
|
||||||
|
|
||||||
|
static telemetrySequenceMarker_t responseToSend;
|
||||||
|
|
||||||
|
static uint8_t ccLen;
|
||||||
|
static uint32_t missingPackets;
|
||||||
|
static uint8_t calData[255][3];
|
||||||
|
static uint8_t cnt;
|
||||||
|
static timeDelta_t t_out;
|
||||||
|
static timeUs_t packet_timer;
|
||||||
|
static uint8_t protocolState;
|
||||||
|
static int16_t word_temp;
|
||||||
|
static uint32_t start_time;
|
||||||
|
|
||||||
|
static bool frame_received;
|
||||||
|
static uint8_t one_time=1;
|
||||||
|
static uint8_t chanskip=1;
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
static uint8_t pass;
|
||||||
|
#endif
|
||||||
|
static timeDelta_t t_received;
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
static uint8_t frame[20];
|
||||||
|
static uint8_t telemetryRX;
|
||||||
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
static uint8_t telemetryOutReader = 0;
|
||||||
|
static uint8_t telemetryOutWriter;
|
||||||
|
|
||||||
|
static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
|
||||||
|
|
||||||
|
static bool telemetryEnabled = false;
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
bool frskySpiDetect(void)//debug CC2500 spi
|
||||||
|
{
|
||||||
|
uint8_t tmp[2];
|
||||||
|
tmp[0] = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST);//Cc2500 read registers chip part num
|
||||||
|
tmp[1] = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST);//Cc2500 read registers chip version
|
||||||
|
if (tmp[0] == 0x80 && tmp[1]==0x03){
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static uint16_t crc(uint8_t *data, uint8_t len) {
|
||||||
|
uint16_t crc = 0;
|
||||||
|
for(uint8_t i=0; i < len; i++)
|
||||||
|
crc = (crc<<8) ^ (CRCTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]);
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
static uint8_t frsky_append_sport_data(uint8_t *buf)
|
||||||
|
{
|
||||||
|
uint8_t index;
|
||||||
|
for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { //max 5 bytes in a frame
|
||||||
|
if(telemetryOutReader == telemetryOutWriter){ //no new data
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
buf[index] = telemetryOutBuffer[telemetryOutReader];
|
||||||
|
telemetryOutReader = (telemetryOutReader + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
}
|
||||||
|
|
||||||
|
return index;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
static void telemetry_build_frame(uint8_t *packet)
|
||||||
|
{
|
||||||
|
frame[0]=0x0E;//length
|
||||||
|
frame[1]=rxFrSkySpiConfig()->bindTxId[0];
|
||||||
|
frame[2]=rxFrSkySpiConfig()->bindTxId[1];
|
||||||
|
frame[3]=packet[3];
|
||||||
|
|
||||||
|
static bool evenRun = false;
|
||||||
|
if (evenRun) {
|
||||||
|
frame[4]=(uint8_t)RSSI_dBm|0x80;
|
||||||
|
} else {
|
||||||
|
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
|
||||||
|
frame[4]=(uint8_t)((adcExternal1Sample & 0xfe0) >> 5); // A1;
|
||||||
|
}
|
||||||
|
evenRun = !evenRun;
|
||||||
|
|
||||||
|
telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
|
||||||
|
telemetrySequenceMarker_t *outFrameMarker = (telemetrySequenceMarker_t *)&frame[5];
|
||||||
|
if (inFrameMarker->data.initRequest) {//check syncronization at startup ok if not no sport telemetry
|
||||||
|
outFrameMarker-> raw = 0;
|
||||||
|
outFrameMarker->data.initRequest = 1;
|
||||||
|
outFrameMarker->data.initResponse = 1;
|
||||||
|
|
||||||
|
localPacketId = 0;
|
||||||
|
} else {
|
||||||
|
if (inFrameMarker->data.retransmissionRequested) {
|
||||||
|
uint8_t retransmissionFrameId = inFrameMarker->data.ackSequenceId;
|
||||||
|
outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
|
||||||
|
outFrameMarker->data.packetSequenceId = retransmissionFrameId;
|
||||||
|
|
||||||
|
memcpy(&frame[6], &telemetryTxBuffer[retransmissionFrameId], TELEMETRY_FRAME_SIZE);
|
||||||
|
} else {
|
||||||
|
uint8_t localAckId = inFrameMarker->data.ackSequenceId;
|
||||||
|
if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) {
|
||||||
|
outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
|
||||||
|
outFrameMarker->data.packetSequenceId = localPacketId;
|
||||||
|
\
|
||||||
|
frame[6] = frsky_append_sport_data(&frame[7]);
|
||||||
|
memcpy(&telemetryTxBuffer[localPacketId], &frame[6], TELEMETRY_FRAME_SIZE);
|
||||||
|
|
||||||
|
localPacketId = (localPacketId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t lcrc = crc(&frame[3], 10);
|
||||||
|
frame[13]=lcrc>>8;
|
||||||
|
frame[14]=lcrc;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool frSkyXCheckQueueEmpty(void)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
static void frSkyXTelemetrySendByte(uint8_t c) {
|
||||||
|
if (c == FSSP_DLE || c == FSSP_START_STOP) {
|
||||||
|
telemetryOutBuffer[telemetryOutWriter] = FSSP_DLE;
|
||||||
|
telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
telemetryOutBuffer[telemetryOutWriter] = c ^ FSSP_DLE_XOR;
|
||||||
|
telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
} else {
|
||||||
|
telemetryOutBuffer[telemetryOutWriter] = c;
|
||||||
|
telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void frSkyXTelemetryWriteFrame(const smartPortPayload_t *payload)
|
||||||
|
{
|
||||||
|
telemetryOutBuffer[telemetryOutWriter] = FSSP_START_STOP;
|
||||||
|
telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
telemetryOutBuffer[telemetryOutWriter] = FSSP_SENSOR_ID1 & 0x1f;
|
||||||
|
telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
|
||||||
|
uint8_t *data = (uint8_t *)payload;
|
||||||
|
for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
|
||||||
|
frSkyXTelemetrySendByte(*data++);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
|
||||||
|
|
||||||
|
static void initialize() {
|
||||||
|
cc2500Reset();
|
||||||
|
cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
|
||||||
|
cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
|
||||||
|
cc2500WriteReg(CC2500_18_MCSM0, 0x18);
|
||||||
|
cc2500WriteReg(CC2500_06_PKTLEN, 0x1E);
|
||||||
|
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04);
|
||||||
|
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01);
|
||||||
|
cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
|
||||||
|
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A);
|
||||||
|
cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
|
||||||
|
cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
|
||||||
|
cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
|
||||||
|
cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
|
||||||
|
cc2500WriteReg(CC2500_10_MDMCFG4, 0x7B);
|
||||||
|
cc2500WriteReg(CC2500_11_MDMCFG3, 0x61);
|
||||||
|
cc2500WriteReg(CC2500_12_MDMCFG2, 0x13);
|
||||||
|
cc2500WriteReg(CC2500_13_MDMCFG1, 0x23);
|
||||||
|
cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A);
|
||||||
|
cc2500WriteReg(CC2500_15_DEVIATN, 0x51);
|
||||||
|
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
|
||||||
|
cc2500WriteReg(CC2500_1A_BSCFG, 0x6C);
|
||||||
|
cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
|
||||||
|
cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
|
||||||
|
cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
|
||||||
|
cc2500WriteReg(CC2500_21_FREND1, 0x56);
|
||||||
|
cc2500WriteReg(CC2500_22_FREND0, 0x10);
|
||||||
|
cc2500WriteReg(CC2500_23_FSCAL3, 0xA9);
|
||||||
|
cc2500WriteReg(CC2500_24_FSCAL2, 0x0A);
|
||||||
|
cc2500WriteReg(CC2500_25_FSCAL1, 0x00);
|
||||||
|
cc2500WriteReg(CC2500_26_FSCAL0, 0x11);
|
||||||
|
cc2500WriteReg(CC2500_29_FSTEST, 0x59);
|
||||||
|
cc2500WriteReg(CC2500_2C_TEST2, 0x88);
|
||||||
|
cc2500WriteReg(CC2500_2D_TEST1, 0x31);
|
||||||
|
cc2500WriteReg(CC2500_2E_TEST0, 0x0B);
|
||||||
|
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
|
||||||
|
cc2500WriteReg(CC2500_09_ADDR, 0x00);
|
||||||
|
cc2500Strobe(CC2500_SIDLE); // Go to idle...
|
||||||
|
|
||||||
|
for(uint8_t c=0;c<0xFF;c++)
|
||||||
|
{//calibrate all channels
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteReg(CC2500_0A_CHANNR, c);
|
||||||
|
cc2500Strobe(CC2500_SCAL);
|
||||||
|
delayMicroseconds(900); //
|
||||||
|
calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
|
||||||
|
calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
|
||||||
|
calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
|
||||||
|
}
|
||||||
|
//#######END INIT########
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
|
||||||
|
{
|
||||||
|
uint16_t c[8];
|
||||||
|
|
||||||
|
c[0] = (uint16_t)((packet[10] <<8)& 0xF00) | packet[9];
|
||||||
|
c[1] = (uint16_t)((packet[11]<<4)&0xFF0) | (packet[10]>>4);
|
||||||
|
c[2] = (uint16_t)((packet[13] <<8)& 0xF00) | packet[12];
|
||||||
|
c[3] = (uint16_t)((packet[14]<<4)&0xFF0) | (packet[13]>>4);
|
||||||
|
c[4] = (uint16_t)((packet[16] <<8)& 0xF00) | packet[15];
|
||||||
|
c[5] = (uint16_t)((packet[17]<<4)&0xFF0) | (packet[16]>>4);
|
||||||
|
c[6] = (uint16_t)((packet[19] <<8)& 0xF00) | packet[18];
|
||||||
|
c[7] = (uint16_t)((packet[20]<<4)&0xFF0) | (packet[19]>>4);
|
||||||
|
|
||||||
|
uint8_t j;
|
||||||
|
for(uint8_t i=0;i<8;i++) {
|
||||||
|
if(c[i] > 2047) {
|
||||||
|
j = 8;
|
||||||
|
c[i] = c[i] - 2048;
|
||||||
|
} else {
|
||||||
|
j = 0;
|
||||||
|
}
|
||||||
|
word_temp = (((c[i]-64)<<1)/3+860);
|
||||||
|
if ((word_temp > 800) && (word_temp < 2200))
|
||||||
|
rcData[i+j] = word_temp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
|
||||||
|
{
|
||||||
|
static unsigned receiveTelemetryRetryCount = 0;
|
||||||
|
static uint32_t polling_time=0;
|
||||||
|
|
||||||
|
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
|
||||||
|
|
||||||
|
switch (protocolState) {
|
||||||
|
case STATE_INIT:
|
||||||
|
if ((millis() - start_time) > 10) {
|
||||||
|
initialize();
|
||||||
|
|
||||||
|
protocolState = STATE_BIND;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_BIND:
|
||||||
|
case STATE_BIND_TUNING:
|
||||||
|
case STATE_BIND_BINDING1:
|
||||||
|
case STATE_BIND_BINDING2:
|
||||||
|
case STATE_BIND_COMPLETE:
|
||||||
|
handleBinding(protocolState, packet);
|
||||||
|
|
||||||
|
break;
|
||||||
|
case STATE_STARTING:
|
||||||
|
listLength = 47;
|
||||||
|
initialiseData(0);
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
nextChannel(1, false); //
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
ret = RX_SPI_RECEIVED_BIND;
|
||||||
|
break;
|
||||||
|
case STATE_UPDATE:
|
||||||
|
packet_timer = micros();
|
||||||
|
protocolState = STATE_DATA;
|
||||||
|
frame_received=false;//again set for receive
|
||||||
|
t_received = 5300;
|
||||||
|
if (checkBindRequested(false)) {
|
||||||
|
packet_timer = 0;
|
||||||
|
t_out = 50;
|
||||||
|
missingPackets = 0;
|
||||||
|
protocolState = STATE_INIT;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// here FS code could be
|
||||||
|
case STATE_DATA:
|
||||||
|
if ((IORead(gdoPin)) &&(frame_received==false)){
|
||||||
|
ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;//read 2 times to avoid reading errors
|
||||||
|
if (ccLen > 32)
|
||||||
|
ccLen = 32;
|
||||||
|
if (ccLen) {
|
||||||
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
uint16_t lcrc= crc(&packet[3],(ccLen-7));
|
||||||
|
if((lcrc >>8)==packet[ccLen-4]&&(lcrc&0x00FF)==packet[ccLen-3]){//check crc
|
||||||
|
if (packet[0] == 0x1D) {
|
||||||
|
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
|
||||||
|
(packet[2] == rxFrSkySpiConfig()->bindTxId[1]) &&
|
||||||
|
(packet[6]==rxFrSkySpiConfig()->rxNum)) {
|
||||||
|
missingPackets = 0;
|
||||||
|
t_out = 1;
|
||||||
|
t_received = 0;
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
if(one_time){
|
||||||
|
chanskip=packet[5]<<2;
|
||||||
|
if(packet[4]<listLength){}
|
||||||
|
else if(packet[4]<(64+listLength))
|
||||||
|
chanskip +=1;
|
||||||
|
else if(packet[4]<(128+listLength))
|
||||||
|
chanskip +=2;
|
||||||
|
else if(packet[4]<(192+listLength))
|
||||||
|
chanskip +=3;
|
||||||
|
telemetryRX=1;//now telemetry can be sent
|
||||||
|
one_time=0;
|
||||||
|
}
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
setRssiDbm(packet[ccLen - 2]);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
|
||||||
|
|
||||||
|
uint8_t remoteNewPacketId = inFrameMarker->data.packetSequenceId;
|
||||||
|
memcpy(&telemetryRxBuffer[remoteNewPacketId].data, &packet[22], TELEMETRY_FRAME_SIZE);
|
||||||
|
telemetryRxBuffer[remoteNewPacketId].needsProcessing = true;
|
||||||
|
|
||||||
|
responseToSend.raw = 0;
|
||||||
|
uint8_t remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
if (remoteNewPacketId != remoteToAckId) {
|
||||||
|
while (remoteToAckId != remoteNewPacketId) {
|
||||||
|
if (!telemetryRxBuffer[remoteToAckId].needsProcessing) {
|
||||||
|
responseToSend.data.ackSequenceId = remoteToAckId;
|
||||||
|
responseToSend.data.retransmissionRequested = 1;
|
||||||
|
|
||||||
|
receiveTelemetryRetryCount++;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!responseToSend.data.retransmissionRequested) {
|
||||||
|
receiveTelemetryRetryCount = 0;
|
||||||
|
|
||||||
|
remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
uint8_t remoteNextAckId;
|
||||||
|
while (telemetryRxBuffer[remoteToAckId].needsProcessing && remoteToAckId != remoteAckId) {
|
||||||
|
remoteNextAckId = remoteToAckId;
|
||||||
|
remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
}
|
||||||
|
remoteAckId = remoteNextAckId;
|
||||||
|
responseToSend.data.ackSequenceId = remoteAckId;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (receiveTelemetryRetryCount >= 5) {
|
||||||
|
remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1;
|
||||||
|
remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1;
|
||||||
|
for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
|
||||||
|
telemetryRxBuffer[i].needsProcessing = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
receiveTelemetryRetryCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
packet_timer=micros();
|
||||||
|
frame_received=true;//no need to process frame again.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (telemetryRX) {
|
||||||
|
if(cmpTimeUs(micros(), packet_timer) > t_received) { // if received or not received in this time sent telemetry data
|
||||||
|
protocolState=STATE_TELEMETRY;
|
||||||
|
telemetry_build_frame(packet);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (cmpTimeUs(micros(), packet_timer) > t_out * SYNC) {
|
||||||
|
if (cnt++ & 0x01) {
|
||||||
|
IOLo(frSkyLedPin);
|
||||||
|
} else {
|
||||||
|
IOHi(frSkyLedPin);
|
||||||
|
}
|
||||||
|
//telemetryTime=micros();
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
|
||||||
|
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
|
||||||
|
#endif
|
||||||
|
nextChannel(1, false);
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
case STATE_TELEMETRY:
|
||||||
|
if(cmpTimeUs(micros(), packet_timer) >= t_received + 400) { // if received or not received in this time sent telemetry data
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500SetPower(6);
|
||||||
|
cc2500Strobe(CC2500_SFRX);
|
||||||
|
delayMicroseconds(30);
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
|
||||||
|
TxEnable();
|
||||||
|
#endif
|
||||||
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
|
cc2500WriteFifo(frame, frame[0] + 1);
|
||||||
|
|
||||||
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
if (telemetryEnabled) {
|
||||||
|
bool clearToSend = false;
|
||||||
|
uint32_t now = millis();
|
||||||
|
smartPortPayload_t *payload = NULL;
|
||||||
|
if ((now - polling_time) > 24) {
|
||||||
|
polling_time=now;
|
||||||
|
|
||||||
|
clearToSend = true;
|
||||||
|
} else {
|
||||||
|
uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
|
||||||
|
while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
|
||||||
|
payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
|
||||||
|
remoteToProcessIndex = remoteToProcessIndex + 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
|
||||||
|
remoteToProcessIndex = 0;
|
||||||
|
telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
|
||||||
|
remoteProcessedId = remoteToProcessId;
|
||||||
|
remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
processSmartPortTelemetry(payload, &clearToSend, NULL);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
protocolState = STATE_RESUME;
|
||||||
|
ret = RX_SPI_RECEIVED_DATA;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
case STATE_RESUME:
|
||||||
|
if (cmpTimeUs(micros(), packet_timer) > t_received + 3700) {
|
||||||
|
packet_timer = micros();
|
||||||
|
t_received=5300;
|
||||||
|
frame_received=false;//again set for receive
|
||||||
|
nextChannel(chanskip, false);
|
||||||
|
cc2500Strobe(CC2500_SRX);
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_PA_LNA
|
||||||
|
RxEnable();
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_DIVERSITY // SE4311 chip
|
||||||
|
if (missingPackets >= 2) {
|
||||||
|
if (pass & 0x01)
|
||||||
|
{
|
||||||
|
IOHi(antSelPin);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
IOLo(antSelPin);
|
||||||
|
}
|
||||||
|
pass++;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#endif // USE_RX_FRSKY_SPI_PA_LNA
|
||||||
|
if (missingPackets > MAX_MISSING_PKT)
|
||||||
|
{
|
||||||
|
t_out = 50;
|
||||||
|
one_time=1;
|
||||||
|
telemetryRX=0;
|
||||||
|
protocolState = STATE_UPDATE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
missingPackets++;
|
||||||
|
protocolState = STATE_DATA;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void frSkyXInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
|
{
|
||||||
|
UNUSED(rxConfig);
|
||||||
|
|
||||||
|
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
|
||||||
|
|
||||||
|
frskySpiRxSetup();
|
||||||
|
|
||||||
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
30
src/main/rx/cc2500_frsky_x.h
Normal file
30
src/main/rx/cc2500_frsky_x.h
Normal file
|
|
@ -0,0 +1,30 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "rx_spi.h"
|
||||||
|
|
||||||
|
struct rxConfig_s;
|
||||||
|
struct rxRuntimeConfig_s;
|
||||||
|
void frSkyXInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
||||||
|
void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload);
|
||||||
|
rx_spi_received_e frSkyXDataReceived(uint8_t *payload);
|
||||||
|
void frSkyXBind();
|
||||||
|
|
@ -310,7 +310,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
#if defined(USE_TELEMETRY_SMARTPORT)
|
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||||
} else {
|
} else {
|
||||||
timeUs_t currentTimeUs = micros();
|
timeUs_t currentTimeUs = micros();
|
||||||
if (clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
|
if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
|
||||||
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
|
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
|
||||||
clearToSend = false;
|
clearToSend = false;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,738 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
|
|
||||||
#ifdef USE_RX_FRSKY_D
|
|
||||||
|
|
||||||
#include "build/build_config.h"
|
|
||||||
#include "build/debug.h"
|
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
#include "common/utils.h"
|
|
||||||
|
|
||||||
#include "drivers/cc2500.h"
|
|
||||||
#include "drivers/io.h"
|
|
||||||
#include "drivers/system.h"
|
|
||||||
#include "drivers/time.h"
|
|
||||||
|
|
||||||
#include "fc/config.h"
|
|
||||||
|
|
||||||
#include "config/feature.h"
|
|
||||||
#include "config/parameter_group_ids.h"
|
|
||||||
|
|
||||||
#include "rx/rx.h"
|
|
||||||
#include "rx/rx_spi.h"
|
|
||||||
#include "rx/frsky_d.h"
|
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
|
||||||
|
|
||||||
#include "telemetry/frsky.h"
|
|
||||||
|
|
||||||
#define RC_CHANNEL_COUNT 8
|
|
||||||
#define MAX_MISSING_PKT 100
|
|
||||||
|
|
||||||
#define DEBUG_DATA_ERROR_COUNT 0
|
|
||||||
|
|
||||||
#define SYNC 9000
|
|
||||||
#define FS_THR 960
|
|
||||||
|
|
||||||
static uint32_t missingPackets;
|
|
||||||
static uint8_t calData[255][3];
|
|
||||||
static uint32_t time_tune;
|
|
||||||
static uint8_t listLength;
|
|
||||||
static uint8_t bindIdx;
|
|
||||||
static uint8_t cnt;
|
|
||||||
static int32_t t_out;
|
|
||||||
static uint8_t Lqi;
|
|
||||||
static timeUs_t lastPacketReceivedTime;
|
|
||||||
static uint8_t protocolState;
|
|
||||||
static uint32_t start_time;
|
|
||||||
static int8_t bindOffset;
|
|
||||||
static uint16_t dataErrorCount = 0;
|
|
||||||
|
|
||||||
static IO_t gdoPin;
|
|
||||||
static IO_t bindPin = DEFIO_IO(NONE);
|
|
||||||
static bool lastBindPinStatus;
|
|
||||||
static IO_t frSkyLedPin;
|
|
||||||
#if defined(USE_FRSKY_RX_PA_LNA)
|
|
||||||
static IO_t txEnPin;
|
|
||||||
static IO_t rxEnPin;
|
|
||||||
static IO_t antSelPin;
|
|
||||||
static uint8_t pass;
|
|
||||||
#endif
|
|
||||||
bool bindRequested = false;
|
|
||||||
|
|
||||||
#ifdef USE_FRSKY_D_TELEMETRY
|
|
||||||
static uint8_t frame[20];
|
|
||||||
static int16_t RSSI_dBm;
|
|
||||||
static uint8_t telemetry_id;
|
|
||||||
static uint32_t telemetryTime;
|
|
||||||
|
|
||||||
#if defined(USE_TELEMETRY_FRSKY)
|
|
||||||
#define MAX_SERIAL_BYTES 64
|
|
||||||
static uint8_t hub_index;
|
|
||||||
static uint8_t idxx = 0;
|
|
||||||
static uint8_t idx_ok = 0;
|
|
||||||
static uint8_t telemetry_expected_id = 0;
|
|
||||||
static 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,
|
|
||||||
.bindTxId = {0, 0},
|
|
||||||
.bindOffset = 0,
|
|
||||||
.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}
|
|
||||||
);
|
|
||||||
|
|
||||||
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_TELEMETRY
|
|
||||||
};
|
|
||||||
|
|
||||||
#if defined(USE_FRSKY_D_TELEMETRY)
|
|
||||||
static void compute_RSSIdbm(uint8_t *packet)
|
|
||||||
{
|
|
||||||
if (packet[18] >= 128) {
|
|
||||||
RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) - 82;
|
|
||||||
} else {
|
|
||||||
RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65;
|
|
||||||
}
|
|
||||||
|
|
||||||
setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(USE_TELEMETRY_FRSKY)
|
|
||||||
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;
|
|
||||||
idxx = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void frSkyTelemetryWriteSpi(uint8_t ch)
|
|
||||||
{
|
|
||||||
if (hub_index < MAX_SERIAL_BYTES) {
|
|
||||||
srx_data[hub_index++] = ch;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static void telemetry_build_frame(uint8_t *packet)
|
|
||||||
{
|
|
||||||
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
|
|
||||||
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
|
|
||||||
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] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
|
|
||||||
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
|
|
||||||
frame[5] = (uint8_t)RSSI_dBm;
|
|
||||||
#if defined(USE_TELEMETRY_FRSKY)
|
|
||||||
bytes_used = frsky_append_hub_data(&frame[8]);
|
|
||||||
#endif
|
|
||||||
frame[6] = bytes_used;
|
|
||||||
frame[7] = telemetry_id;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(USE_FRSKY_RX_PA_LNA)
|
|
||||||
static void RX_enable(void)
|
|
||||||
{
|
|
||||||
IOLo(txEnPin);
|
|
||||||
IOHi(rxEnPin);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void TX_enable(void)
|
|
||||||
{
|
|
||||||
IOLo(rxEnPin);
|
|
||||||
IOHi(txEnPin);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void frSkyDBind(void)
|
|
||||||
{
|
|
||||||
bindRequested = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void initialize(void)
|
|
||||||
{
|
|
||||||
cc2500Reset();
|
|
||||||
cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
|
|
||||||
cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
|
|
||||||
cc2500WriteReg(CC2500_18_MCSM0, 0x18);
|
|
||||||
cc2500WriteReg(CC2500_06_PKTLEN, 0x19);
|
|
||||||
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
|
|
||||||
cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
|
|
||||||
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
|
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
|
|
||||||
cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
|
|
||||||
cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
|
|
||||||
cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
|
|
||||||
cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA);
|
|
||||||
cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
|
|
||||||
cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
|
|
||||||
cc2500WriteReg(CC2500_13_MDMCFG1, 0x23);
|
|
||||||
cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A);
|
|
||||||
cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
|
|
||||||
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
|
|
||||||
cc2500WriteReg(CC2500_1A_BSCFG, 0x6C);
|
|
||||||
cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
|
|
||||||
cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
|
|
||||||
cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
|
|
||||||
cc2500WriteReg(CC2500_21_FREND1, 0x56);
|
|
||||||
cc2500WriteReg(CC2500_22_FREND0, 0x10);
|
|
||||||
cc2500WriteReg(CC2500_23_FSCAL3, 0xA9);
|
|
||||||
cc2500WriteReg(CC2500_24_FSCAL2, 0x0A);
|
|
||||||
cc2500WriteReg(CC2500_25_FSCAL1, 0x00);
|
|
||||||
cc2500WriteReg(CC2500_26_FSCAL0, 0x11);
|
|
||||||
cc2500WriteReg(CC2500_29_FSTEST, 0x59);
|
|
||||||
cc2500WriteReg(CC2500_2C_TEST2, 0x88);
|
|
||||||
cc2500WriteReg(CC2500_2D_TEST1, 0x31);
|
|
||||||
cc2500WriteReg(CC2500_2E_TEST0, 0x0B);
|
|
||||||
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
|
|
||||||
cc2500WriteReg(CC2500_09_ADDR, 0x00);
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
|
|
||||||
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04);
|
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, 0);
|
|
||||||
for (uint8_t c = 0; c < 0xFF; c++) {
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500WriteReg(CC2500_0A_CHANNR, c);
|
|
||||||
cc2500Strobe(CC2500_SCAL);
|
|
||||||
delayMicroseconds(900);
|
|
||||||
calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
|
|
||||||
calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
|
|
||||||
calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void initialize_data(uint8_t adr)
|
|
||||||
{
|
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset);
|
|
||||||
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
|
|
||||||
cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]);
|
|
||||||
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
|
|
||||||
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
|
|
||||||
delay(10);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void initTuneRx(void)
|
|
||||||
{
|
|
||||||
cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
|
|
||||||
time_tune = millis();
|
|
||||||
bindOffset = -126;
|
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
|
||||||
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
|
|
||||||
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
|
|
||||||
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
|
|
||||||
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
|
|
||||||
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
|
|
||||||
cc2500WriteReg(CC2500_0A_CHANNR, 0);
|
|
||||||
cc2500Strobe(CC2500_SFRX);
|
|
||||||
cc2500Strobe(CC2500_SRX);
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool tuneRx(uint8_t *packet)
|
|
||||||
{
|
|
||||||
if (bindOffset >= 126) {
|
|
||||||
bindOffset = -126;
|
|
||||||
}
|
|
||||||
if ((millis() - time_tune) > 50) {
|
|
||||||
time_tune = millis();
|
|
||||||
bindOffset += 5;
|
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
|
||||||
}
|
|
||||||
if (IORead(gdoPin)) {
|
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
||||||
if (ccLen) {
|
|
||||||
cc2500ReadFifo(packet, ccLen);
|
|
||||||
if (packet[ccLen - 1] & 0x80) {
|
|
||||||
if (packet[2] == 0x01) {
|
|
||||||
Lqi = packet[ccLen - 1] & 0x7F;
|
|
||||||
if (Lqi < 50) {
|
|
||||||
frSkyDConfigMutable()->bindOffset = bindOffset;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void initGetBind(void)
|
|
||||||
{
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
|
|
||||||
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
|
|
||||||
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
|
|
||||||
cc2500WriteReg(CC2500_0A_CHANNR, 0);
|
|
||||||
cc2500Strobe(CC2500_SFRX);
|
|
||||||
delayMicroseconds(20); // waiting flush FIFO
|
|
||||||
|
|
||||||
cc2500Strobe(CC2500_SRX);
|
|
||||||
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 (IORead(gdoPin)) {
|
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
||||||
if (ccLen) {
|
|
||||||
cc2500ReadFifo(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 (IORead(gdoPin)) {
|
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
||||||
if (ccLen) {
|
|
||||||
cc2500ReadFifo(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;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
for (uint8_t n = 0; n < 5; n++) {
|
|
||||||
if (packet[6 + n] == packet[ccLen - 3] ||
|
|
||||||
(packet[6 + n] == 0)) {
|
|
||||||
if (bindIdx >= 0x2D) {
|
|
||||||
listLength = packet[5] + n;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
frSkyDConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n];
|
|
||||||
}
|
|
||||||
|
|
||||||
bindIdx = bindIdx + 5;
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
} else {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
static void nextChannel(uint8_t skip)
|
|
||||||
{
|
|
||||||
static uint8_t channr = 0;
|
|
||||||
|
|
||||||
channr += skip;
|
|
||||||
while (channr >= listLength) {
|
|
||||||
channr -= listLength;
|
|
||||||
}
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500WriteReg(CC2500_23_FSCAL3,
|
|
||||||
calData[frSkyDConfig()->bindHopData[channr]][0]);
|
|
||||||
cc2500WriteReg(CC2500_24_FSCAL2,
|
|
||||||
calData[frSkyDConfig()->bindHopData[channr]][1]);
|
|
||||||
cc2500WriteReg(CC2500_25_FSCAL1,
|
|
||||||
calData[frSkyDConfig()->bindHopData[channr]][2]);
|
|
||||||
cc2500WriteReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]);
|
|
||||||
cc2500Strobe(CC2500_SFRX);
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool checkBindRequested(bool reset)
|
|
||||||
{
|
|
||||||
if (bindPin) {
|
|
||||||
bool bindPinStatus = IORead(bindPin);
|
|
||||||
if (lastBindPinStatus && !bindPinStatus) {
|
|
||||||
bindRequested = true;
|
|
||||||
}
|
|
||||||
lastBindPinStatus = bindPinStatus;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!bindRequested) {
|
|
||||||
return false;
|
|
||||||
} else {
|
|
||||||
if (reset) {
|
|
||||||
bindRequested = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
|
|
||||||
|
|
||||||
static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
|
|
||||||
channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
|
|
||||||
channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
|
|
||||||
{
|
|
||||||
uint16_t channels[RC_CHANNEL_COUNT];
|
|
||||||
bool dataError = false;
|
|
||||||
|
|
||||||
decodeChannelPair(channels, packet + 6, 4);
|
|
||||||
decodeChannelPair(channels + 2, packet + 8, 3);
|
|
||||||
decodeChannelPair(channels + 4, packet + 12, 4);
|
|
||||||
decodeChannelPair(channels + 6, packet + 14, 3);
|
|
||||||
|
|
||||||
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
|
|
||||||
if ((channels[i] < 800) || (channels[i] > 2200)) {
|
|
||||||
dataError = true;
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!dataError) {
|
|
||||||
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
|
|
||||||
rcData[i] = channels[i];
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DEBUG_SET(DEBUG_FRSKY_D_RX, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
|
|
||||||
{
|
|
||||||
const timeUs_t currentPacketReceivedTime = micros();
|
|
||||||
|
|
||||||
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
|
|
||||||
|
|
||||||
switch (protocolState) {
|
|
||||||
case STATE_INIT:
|
|
||||||
if ((millis() - start_time) > 10) {
|
|
||||||
initialize();
|
|
||||||
|
|
||||||
protocolState = STATE_BIND;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_BIND:
|
|
||||||
if (checkBindRequested(true) || frSkyDConfig()->autoBind) {
|
|
||||||
IOHi(frSkyLedPin);
|
|
||||||
initTuneRx();
|
|
||||||
|
|
||||||
protocolState = STATE_BIND_TUNING;
|
|
||||||
} else {
|
|
||||||
protocolState = STATE_STARTING;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_BIND_TUNING:
|
|
||||||
if (tuneRx(packet)) {
|
|
||||||
initGetBind();
|
|
||||||
initialize_data(1);
|
|
||||||
|
|
||||||
protocolState = STATE_BIND_BINDING1;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_BIND_BINDING1:
|
|
||||||
if (getBind1(packet)) {
|
|
||||||
protocolState = STATE_BIND_BINDING2;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_BIND_BINDING2:
|
|
||||||
if (getBind2(packet)) {
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
|
|
||||||
protocolState = STATE_BIND_COMPLETE;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_BIND_COMPLETE:
|
|
||||||
if (!frSkyDConfig()->autoBind) {
|
|
||||||
writeEEPROM();
|
|
||||||
} else {
|
|
||||||
uint8_t ctr = 40;
|
|
||||||
while (ctr--) {
|
|
||||||
IOHi(frSkyLedPin);
|
|
||||||
delay(50);
|
|
||||||
IOLo(frSkyLedPin);
|
|
||||||
delay(50);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
protocolState = STATE_STARTING;
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_STARTING:
|
|
||||||
listLength = 47;
|
|
||||||
initialize_data(0);
|
|
||||||
protocolState = STATE_UPDATE;
|
|
||||||
nextChannel(1); //
|
|
||||||
cc2500Strobe(CC2500_SRX);
|
|
||||||
ret = RX_SPI_RECEIVED_BIND;
|
|
||||||
|
|
||||||
break;
|
|
||||||
case STATE_UPDATE:
|
|
||||||
lastPacketReceivedTime = currentPacketReceivedTime;
|
|
||||||
protocolState = STATE_DATA;
|
|
||||||
|
|
||||||
if (checkBindRequested(false)) {
|
|
||||||
lastPacketReceivedTime = 0;
|
|
||||||
t_out = 50;
|
|
||||||
missingPackets = 0;
|
|
||||||
|
|
||||||
protocolState = STATE_INIT;
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// here FS code could be
|
|
||||||
case STATE_DATA:
|
|
||||||
if (IORead(gdoPin)) {
|
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
||||||
if (ccLen >= 20) {
|
|
||||||
cc2500ReadFifo(packet, 20);
|
|
||||||
if (packet[19] & 0x80) {
|
|
||||||
missingPackets = 0;
|
|
||||||
t_out = 1;
|
|
||||||
if (packet[0] == 0x11) {
|
|
||||||
if ((packet[1] == frSkyDConfig()->bindTxId[0]) &&
|
|
||||||
(packet[2] == frSkyDConfig()->bindTxId[1])) {
|
|
||||||
IOHi(frSkyLedPin);
|
|
||||||
nextChannel(1);
|
|
||||||
#ifdef USE_FRSKY_D_TELEMETRY
|
|
||||||
if ((packet[3] % 4) == 2) {
|
|
||||||
telemetryTime = micros();
|
|
||||||
compute_RSSIdbm(packet);
|
|
||||||
telemetry_build_frame(packet);
|
|
||||||
protocolState = STATE_TELEMETRY;
|
|
||||||
} else
|
|
||||||
#endif
|
|
||||||
{
|
|
||||||
cc2500Strobe(CC2500_SRX);
|
|
||||||
protocolState = STATE_UPDATE;
|
|
||||||
}
|
|
||||||
ret = RX_SPI_RECEIVED_DATA;
|
|
||||||
lastPacketReceivedTime = currentPacketReceivedTime;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) {
|
|
||||||
#ifdef USE_FRSKY_RX_PA_LNA
|
|
||||||
RX_enable();
|
|
||||||
#endif
|
|
||||||
if (t_out == 1) {
|
|
||||||
#ifdef USE_FRSKY_RX_DIVERSITY // SE4311 chip
|
|
||||||
if (missingPackets >= 2) {
|
|
||||||
if (pass & 0x01) {
|
|
||||||
IOHi(antSelPin);
|
|
||||||
} else {
|
|
||||||
IOLo(antSelPin);
|
|
||||||
}
|
|
||||||
pass++;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (missingPackets > MAX_MISSING_PKT)
|
|
||||||
t_out = 50;
|
|
||||||
|
|
||||||
missingPackets++;
|
|
||||||
nextChannel(1);
|
|
||||||
} else {
|
|
||||||
if (cnt++ & 0x01) {
|
|
||||||
IOLo(frSkyLedPin);
|
|
||||||
} else
|
|
||||||
IOHi(frSkyLedPin);
|
|
||||||
|
|
||||||
nextChannel(13);
|
|
||||||
}
|
|
||||||
|
|
||||||
cc2500Strobe(CC2500_SRX);
|
|
||||||
protocolState = STATE_UPDATE;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
#ifdef USE_FRSKY_D_TELEMETRY
|
|
||||||
case STATE_TELEMETRY:
|
|
||||||
if ((micros() - telemetryTime) >= 1380) {
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500SetPower(6);
|
|
||||||
cc2500Strobe(CC2500_SFRX);
|
|
||||||
#if defined(USE_FRSKY_RX_PA_LNA)
|
|
||||||
TX_enable();
|
|
||||||
#endif
|
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
|
||||||
cc2500WriteFifo(frame, frame[0] + 1);
|
|
||||||
protocolState = STATE_DATA;
|
|
||||||
ret = RX_SPI_RECEIVED_DATA;
|
|
||||||
lastPacketReceivedTime = currentPacketReceivedTime;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void frskyD_Rx_Setup(rx_spi_protocol_e protocol)
|
|
||||||
{
|
|
||||||
UNUSED(protocol);
|
|
||||||
// gpio init here
|
|
||||||
gdoPin = IOGetByTag(IO_TAG(FRSKY_RX_GDO_0_PIN));
|
|
||||||
IOInit(gdoPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
|
|
||||||
frSkyLedPin = IOGetByTag(IO_TAG(FRSKY_RX_LED_PIN));
|
|
||||||
IOInit(frSkyLedPin, OWNER_LED, 0);
|
|
||||||
IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP);
|
|
||||||
#if defined(USE_FRSKY_RX_PA_LNA)
|
|
||||||
rxEnPin = IOGetByTag(IO_TAG(FRSKY_RX_RX_EN_PIN));
|
|
||||||
IOInit(rxEnPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(rxEnPin, IOCFG_OUT_PP);
|
|
||||||
txEnPin = IOGetByTag(IO_TAG(FRSKY_RX_TX_EN_PIN));
|
|
||||||
IOInit(txEnPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(txEnPin, IOCFG_OUT_PP);
|
|
||||||
#endif
|
|
||||||
#if defined(USE_FRSKY_RX_DIVERSITY)
|
|
||||||
antSelPin = IOGetByTag(IO_TAG(FRSKY_RX_ANT_SEL_PIN));
|
|
||||||
IOInit(antSelPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
|
|
||||||
#endif
|
|
||||||
#if defined(BINDPLUG_PIN)
|
|
||||||
bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
|
|
||||||
IOInit(bindPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(bindPin, IOCFG_IPU);
|
|
||||||
|
|
||||||
lastBindPinStatus = IORead(bindPin);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
start_time = millis();
|
|
||||||
lastPacketReceivedTime = 0;
|
|
||||||
t_out = 50;
|
|
||||||
missingPackets = 0;
|
|
||||||
protocolState = STATE_INIT;
|
|
||||||
#if defined(USE_FRSKY_RX_DIVERSITY)
|
|
||||||
IOHi(antSelPin);
|
|
||||||
#endif
|
|
||||||
#if defined(USE_FRSKY_RX_PA_LNA)
|
|
||||||
RX_enable();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(USE_FRSKY_D_TELEMETRY)
|
|
||||||
#if defined(USE_TELEMETRY_FRSKY)
|
|
||||||
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
|
|
||||||
&frSkyTelemetryWriteSpi);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (rssiSource == RSSI_SOURCE_NONE) {
|
|
||||||
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// if(!frSkySpiDetect())//detect spi working routine
|
|
||||||
// return;
|
|
||||||
}
|
|
||||||
|
|
||||||
void frSkyDInit(const rxConfig_t *rxConfig,
|
|
||||||
rxRuntimeConfig_t *rxRuntimeConfig)
|
|
||||||
{
|
|
||||||
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
|
|
||||||
frskyD_Rx_Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
@ -35,7 +35,8 @@
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/rx_spi.h"
|
#include "rx/rx_spi.h"
|
||||||
#include "rx/frsky_d.h"
|
#include "cc2500_frsky_d.h"
|
||||||
|
#include "cc2500_frsky_x.h"
|
||||||
#include "rx/nrf24_cx10.h"
|
#include "rx/nrf24_cx10.h"
|
||||||
#include "rx/nrf24_syma.h"
|
#include "rx/nrf24_syma.h"
|
||||||
#include "rx/nrf24_v202.h"
|
#include "rx/nrf24_v202.h"
|
||||||
|
|
@ -113,13 +114,20 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
||||||
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
|
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_RX_FRSKY_D
|
#ifdef USE_RX_FRSKY_SPI_D
|
||||||
case RX_SPI_FRSKY_D:
|
case RX_SPI_FRSKY_D:
|
||||||
protocolInit = frSkyDInit;
|
protocolInit = frSkyDInit;
|
||||||
protocolDataReceived = frSkyDDataReceived;
|
protocolDataReceived = frSkyDDataReceived;
|
||||||
protocolSetRcDataFromPayload = frSkyDSetRcData;
|
protocolSetRcDataFromPayload = frSkyDSetRcData;
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_RX_FRSKY_SPI_X
|
||||||
|
case RX_SPI_FRSKY_X:
|
||||||
|
protocolInit = frSkyXInit;
|
||||||
|
protocolDataReceived = frSkyXDataReceived;
|
||||||
|
protocolSetRcDataFromPayload = frSkyXSetRcData;
|
||||||
|
break;
|
||||||
|
#endif
|
||||||
#ifdef USE_RX_FLYSKY
|
#ifdef USE_RX_FLYSKY
|
||||||
case RX_SPI_A7105_FLYSKY:
|
case RX_SPI_A7105_FLYSKY:
|
||||||
case RX_SPI_A7105_FLYSKY_2A:
|
case RX_SPI_A7105_FLYSKY_2A:
|
||||||
|
|
|
||||||
|
|
@ -30,6 +30,7 @@ typedef enum {
|
||||||
RX_SPI_NRF24_H8_3D,
|
RX_SPI_NRF24_H8_3D,
|
||||||
RX_SPI_NRF24_INAV,
|
RX_SPI_NRF24_INAV,
|
||||||
RX_SPI_FRSKY_D,
|
RX_SPI_FRSKY_D,
|
||||||
|
RX_SPI_FRSKY_X,
|
||||||
RX_SPI_A7105_FLYSKY,
|
RX_SPI_A7105_FLYSKY,
|
||||||
RX_SPI_A7105_FLYSKY_2A,
|
RX_SPI_A7105_FLYSKY_2A,
|
||||||
RX_SPI_PROTOCOL_COUNT
|
RX_SPI_PROTOCOL_COUNT
|
||||||
|
|
|
||||||
|
|
@ -99,25 +99,33 @@
|
||||||
#define RX_SPI_INSTANCE SPI1
|
#define RX_SPI_INSTANCE SPI1
|
||||||
#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
|
#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
|
||||||
|
|
||||||
#define USE_RX_FRSKY_D
|
|
||||||
|
#define USE_RX_FRSKY_SPI_D
|
||||||
|
#define USE_RX_FRSKY_SPI_X
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
|
||||||
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_D
|
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X
|
||||||
|
#define USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
#define USE_FRSKY_D_TELEMETRY
|
|
||||||
|
|
||||||
#define USE_FRSKY_RX_PA_LNA
|
|
||||||
#define USE_FRSKY_RX_DIVERSITY
|
|
||||||
|
|
||||||
#define RX_NSS_PIN SPI1_NSS_PIN
|
#define RX_NSS_PIN SPI1_NSS_PIN
|
||||||
#define RX_SCK_PIN SPI1_SCK_PIN
|
#define RX_SCK_PIN SPI1_SCK_PIN
|
||||||
#define RX_MISO_PIN SPI1_MISO_PIN
|
#define RX_MISO_PIN SPI1_MISO_PIN
|
||||||
#define RX_MOSI_PIN SPI1_MOSI_PIN
|
#define RX_MOSI_PIN SPI1_MOSI_PIN
|
||||||
|
|
||||||
#define FRSKY_RX_GDO_0_PIN PB0
|
#define RX_FRSKY_SPI_GDO_0_PIN PB0
|
||||||
#define FRSKY_RX_ANT_SEL_PIN PB2
|
|
||||||
#define FRSKY_RX_TX_EN_PIN PB1
|
#define RX_FRSKY_SPI_LED_PIN PB6
|
||||||
#define FRSKY_RX_RX_EN_PIN PB11
|
|
||||||
#define FRSKY_RX_LED_PIN PB6
|
|
||||||
|
#define USE_RX_FRSKY_SPI_PA_LNA
|
||||||
|
|
||||||
|
#define RX_FRSKY_SPI_TX_EN_PIN PB1
|
||||||
|
#define RX_FRSKY_SPI_LNA_EN_PIN PB11
|
||||||
|
|
||||||
|
|
||||||
|
#define USE_RX_FRSKY_SPI_DIVERSITY
|
||||||
|
|
||||||
|
#define RX_FRSKY_SPI_ANT_SEL_PIN PB2
|
||||||
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PC13
|
#define BINDPLUG_PIN PC13
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -6,4 +6,6 @@ TARGET_SRC = \
|
||||||
drivers/accgyro/accgyro_mpu.c \
|
drivers/accgyro/accgyro_mpu.c \
|
||||||
drivers/accgyro/accgyro_mpu6050.c \
|
drivers/accgyro/accgyro_mpu6050.c \
|
||||||
drivers/cc2500.c \
|
drivers/cc2500.c \
|
||||||
rx/frsky_d.c
|
rx/cc2500_frsky_shared.c \
|
||||||
|
rx/cc2500_frsky_d.c \
|
||||||
|
rx/cc2500_frsky_x.c
|
||||||
|
|
|
||||||
|
|
@ -73,3 +73,8 @@
|
||||||
#undef VTX_TRAMP
|
#undef VTX_TRAMP
|
||||||
#undef VTX_SMARTAUDIO
|
#undef VTX_SMARTAUDIO
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X)
|
||||||
|
#define USE_RX_FRSKY_SPI
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -132,6 +132,7 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
|
||||||
sbufAdvance(frameBuf, frameBytesRemaining);
|
sbufAdvance(frameBuf, frameBytesRemaining);
|
||||||
sbufWriteData(rxBuf, payload, frameBytesRemaining);
|
sbufWriteData(rxBuf, payload, frameBytesRemaining);
|
||||||
lastSeq = seqNumber;
|
lastSeq = seqNumber;
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
} else {
|
} else {
|
||||||
sbufReadData(frameBuf, payload, bufferBytesRemaining);
|
sbufReadData(frameBuf, payload, bufferBytesRemaining);
|
||||||
|
|
|
||||||
|
|
@ -10,7 +10,7 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#ifdef USE_TELEMETRY
|
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
|
@ -65,27 +65,6 @@ enum
|
||||||
SPSTATE_INITIALIZED_EXTERNAL,
|
SPSTATE_INITIALIZED_EXTERNAL,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum
|
|
||||||
{
|
|
||||||
FSSP_START_STOP = 0x7E,
|
|
||||||
|
|
||||||
FSSP_DLE = 0x7D,
|
|
||||||
FSSP_DLE_XOR = 0x20,
|
|
||||||
|
|
||||||
FSSP_DATA_FRAME = 0x10,
|
|
||||||
FSSP_MSPC_FRAME_SMARTPORT = 0x30, // MSP client frame
|
|
||||||
FSSP_MSPC_FRAME_FPORT = 0x31, // MSP client frame
|
|
||||||
FSSP_MSPS_FRAME = 0x32, // MSP server frame
|
|
||||||
|
|
||||||
// ID of sensor. Must be something that is polled by FrSky RX
|
|
||||||
FSSP_SENSOR_ID1 = 0x1B,
|
|
||||||
FSSP_SENSOR_ID2 = 0x0D,
|
|
||||||
FSSP_SENSOR_ID3 = 0x34,
|
|
||||||
FSSP_SENSOR_ID4 = 0x67
|
|
||||||
// there are 32 ID's polled by smartport master
|
|
||||||
// remaining 3 bits are crc (according to comments in openTx code)
|
|
||||||
};
|
|
||||||
|
|
||||||
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
|
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
|
||||||
enum
|
enum
|
||||||
{
|
{
|
||||||
|
|
@ -166,7 +145,7 @@ static smartPortWriteFrameFn *smartPortWriteFrame;
|
||||||
static bool smartPortMspReplyPending = false;
|
static bool smartPortMspReplyPending = false;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToSend)
|
smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
|
||||||
{
|
{
|
||||||
static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
|
static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
|
||||||
static uint8_t smartPortRxBytes = 0;
|
static uint8_t smartPortRxBytes = 0;
|
||||||
|
|
@ -188,7 +167,7 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS
|
||||||
|
|
||||||
if (awaitingSensorId) {
|
if (awaitingSensorId) {
|
||||||
awaitingSensorId = false;
|
awaitingSensorId = false;
|
||||||
if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
|
if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
|
||||||
// our slot is starting, no need to decode more
|
// our slot is starting, no need to decode more
|
||||||
*clearToSend = true;
|
*clearToSend = true;
|
||||||
skipUntilStart = true;
|
skipUntilStart = true;
|
||||||
|
|
@ -210,6 +189,12 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS
|
||||||
if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
|
if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
|
||||||
rxBuffer[smartPortRxBytes++] = (uint8_t)c;
|
rxBuffer[smartPortRxBytes++] = (uint8_t)c;
|
||||||
checksum += c;
|
checksum += c;
|
||||||
|
|
||||||
|
if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
|
||||||
|
skipUntilStart = true;
|
||||||
|
|
||||||
|
return (smartPortPayload_t *)&rxBuffer;
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
skipUntilStart = true;
|
skipUntilStart = true;
|
||||||
|
|
||||||
|
|
@ -386,17 +371,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
||||||
static uint8_t t2Cnt = 0;
|
static uint8_t t2Cnt = 0;
|
||||||
|
|
||||||
switch (id) {
|
switch (id) {
|
||||||
#ifdef USE_GPS
|
|
||||||
case FSSP_DATAID_SPEED :
|
|
||||||
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
|
||||||
//convert to knots: 1cm/s = 0.0194384449 knots
|
|
||||||
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
|
|
||||||
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
|
|
||||||
smartPortSendPackage(id, tmpui);
|
|
||||||
*clearToSend = false;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
case FSSP_DATAID_VFAS :
|
case FSSP_DATAID_VFAS :
|
||||||
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
|
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
|
||||||
uint16_t vfasVoltage;
|
uint16_t vfasVoltage;
|
||||||
|
|
@ -430,28 +404,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
||||||
break;
|
break;
|
||||||
//case FSSP_DATAID_ADC1 :
|
//case FSSP_DATAID_ADC1 :
|
||||||
//case FSSP_DATAID_ADC2 :
|
//case FSSP_DATAID_ADC2 :
|
||||||
#ifdef USE_GPS
|
|
||||||
case FSSP_DATAID_LATLONG :
|
|
||||||
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
|
||||||
uint32_t tmpui = 0;
|
|
||||||
// the same ID is sent twice, one for longitude, one for latitude
|
|
||||||
// the MSB of the sent uint32_t helps FrSky keep track
|
|
||||||
// the even/odd bit of our counter helps us keep track
|
|
||||||
if (smartPortIdCnt & 1) {
|
|
||||||
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
|
|
||||||
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
|
|
||||||
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
|
|
||||||
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
|
|
||||||
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
|
|
||||||
}
|
|
||||||
smartPortSendPackage(id, tmpui);
|
|
||||||
*clearToSend = false;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
//case FSSP_DATAID_CAP_USED :
|
//case FSSP_DATAID_CAP_USED :
|
||||||
case FSSP_DATAID_VARIO :
|
case FSSP_DATAID_VARIO :
|
||||||
if (sensors(SENSOR_BARO)) {
|
if (sensors(SENSOR_BARO)) {
|
||||||
|
|
@ -564,6 +516,35 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
#ifdef USE_GPS
|
#ifdef USE_GPS
|
||||||
|
case FSSP_DATAID_SPEED :
|
||||||
|
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
||||||
|
//convert to knots: 1cm/s = 0.0194384449 knots
|
||||||
|
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
|
||||||
|
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
|
||||||
|
smartPortSendPackage(id, tmpui);
|
||||||
|
*clearToSend = false;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case FSSP_DATAID_LATLONG :
|
||||||
|
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
||||||
|
uint32_t tmpui = 0;
|
||||||
|
// the same ID is sent twice, one for longitude, one for latitude
|
||||||
|
// the MSB of the sent uint32_t helps FrSky keep track
|
||||||
|
// the even/odd bit of our counter helps us keep track
|
||||||
|
if (smartPortIdCnt & 1) {
|
||||||
|
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
|
||||||
|
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
|
||||||
|
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
|
||||||
|
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
|
||||||
|
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
|
||||||
|
}
|
||||||
|
smartPortSendPackage(id, tmpui);
|
||||||
|
*clearToSend = false;
|
||||||
|
}
|
||||||
|
break;
|
||||||
case FSSP_DATAID_GPS_ALT :
|
case FSSP_DATAID_GPS_ALT :
|
||||||
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
||||||
smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
|
smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
|
||||||
|
|
@ -584,6 +565,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool serialCheckQueueEmpty(void)
|
||||||
|
{
|
||||||
|
return (serialRxBytesWaiting(smartPortSerialPort) == 0);
|
||||||
|
}
|
||||||
|
|
||||||
void handleSmartPortTelemetry(void)
|
void handleSmartPortTelemetry(void)
|
||||||
{
|
{
|
||||||
static bool clearToSend = false;
|
static bool clearToSend = false;
|
||||||
|
|
@ -597,7 +583,7 @@ void handleSmartPortTelemetry(void)
|
||||||
smartPortPayload_t *payload = NULL;
|
smartPortPayload_t *payload = NULL;
|
||||||
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
|
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
|
||||||
uint8_t c = serialRead(smartPortSerialPort);
|
uint8_t c = serialRead(smartPortSerialPort);
|
||||||
payload = smartPortDataReceiveSerial(c, &clearToSend);
|
payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
|
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
|
||||||
|
|
|
||||||
|
|
@ -13,6 +13,27 @@
|
||||||
#define SMARTPORT_MSP_TX_BUF_SIZE 256
|
#define SMARTPORT_MSP_TX_BUF_SIZE 256
|
||||||
#define SMARTPORT_MSP_RX_BUF_SIZE 64
|
#define SMARTPORT_MSP_RX_BUF_SIZE 64
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
FSSP_START_STOP = 0x7E,
|
||||||
|
|
||||||
|
FSSP_DLE = 0x7D,
|
||||||
|
FSSP_DLE_XOR = 0x20,
|
||||||
|
|
||||||
|
FSSP_DATA_FRAME = 0x10,
|
||||||
|
FSSP_MSPC_FRAME_SMARTPORT = 0x30, // MSP client frame
|
||||||
|
FSSP_MSPC_FRAME_FPORT = 0x31, // MSP client frame
|
||||||
|
FSSP_MSPS_FRAME = 0x32, // MSP server frame
|
||||||
|
|
||||||
|
// ID of sensor. Must be something that is polled by FrSky RX
|
||||||
|
FSSP_SENSOR_ID1 = 0x1B,
|
||||||
|
FSSP_SENSOR_ID2 = 0x0D,
|
||||||
|
FSSP_SENSOR_ID3 = 0x34,
|
||||||
|
FSSP_SENSOR_ID4 = 0x67
|
||||||
|
// there are 32 ID's polled by smartport master
|
||||||
|
// remaining 3 bits are crc (according to comments in openTx code)
|
||||||
|
};
|
||||||
|
|
||||||
typedef struct smartPortPayload_s {
|
typedef struct smartPortPayload_s {
|
||||||
uint8_t frameId;
|
uint8_t frameId;
|
||||||
uint16_t valueId;
|
uint16_t valueId;
|
||||||
|
|
@ -20,6 +41,7 @@ typedef struct smartPortPayload_s {
|
||||||
} __attribute__((packed)) smartPortPayload_t;
|
} __attribute__((packed)) smartPortPayload_t;
|
||||||
|
|
||||||
typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
|
typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
|
||||||
|
typedef bool smartPortCheckQueueEmptyFn(void);
|
||||||
|
|
||||||
bool initSmartPortTelemetry(void);
|
bool initSmartPortTelemetry(void);
|
||||||
void checkSmartPortTelemetryState(void);
|
void checkSmartPortTelemetryState(void);
|
||||||
|
|
@ -28,6 +50,8 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx
|
||||||
void handleSmartPortTelemetry(void);
|
void handleSmartPortTelemetry(void);
|
||||||
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout);
|
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout);
|
||||||
|
|
||||||
|
smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum);
|
||||||
|
|
||||||
struct serialPort_s;
|
struct serialPort_s;
|
||||||
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);
|
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);
|
||||||
void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);
|
void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue