rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added FrSky X SPI RX protocol. 

Original implementation from midelic.

Added RX number support.

Fixed (almost) SmartPort over SPI.

Fixed indentation.

Somewhat working telemetry.

Fixed SmartPort.

Work on SmartPort.

Work on SmartPort.

Working version without RX ringbuffer.

Na, stuff it, ringbuffer is better.

Fixed build.

Make sure we don't lose packets.

Made MSP over SmartPort over SPI work.

Moved processing of incoming telemetry into 'handleTelemetry'.

Improved telemetry buffering.

Make sure telemetry polling is happening.

Some cleanups.

Make telemetry wait if MSP is pemding.

Made MSP over SmartPort work.

Fixes after rebase.

Combined FrSky D and FrSky X.

Combined FrSky D and FrSky X.

Merged D and X.
This commit is contained in:
mikeller 2017-09-22 00:32:13 +12:00
parent a0c672ba69
commit d3a6b3730f
22 changed files with 1708 additions and 848 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/build/debug.c
View File

@ -50,7 +50,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"FFT",
"FFT_TIME",
"FFT_FREQ",
"FRSKY_D_RX",
"RX_FRSKY_SPI",
"GYRO_RAW",
"MAX7456_SIGNAL",
"MAX7456_SPICLOCK",

2
src/main/build/debug.h
View File

@ -68,7 +68,7 @@ typedef enum {
DEBUG_FFT,
DEBUG_FFT_TIME,
DEBUG_FFT_FREQ,
DEBUG_FRSKY_D_RX,
DEBUG_RX_FRSKY_SPI,
DEBUG_GYRO_RAW,
DEBUG_MAX7456_SIGNAL,
DEBUG_MAX7456_SPICLOCK,

2
src/main/config/parameter_group_ids.h
View File

@ -113,7 +113,7 @@
#define PG_SPI_PIN_CONFIG 520
#define PG_ESCSERIAL_CONFIG 521
#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_FLYSKY_CONFIG 525
#define PG_TIME_CONFIG 526

25
src/main/interface/cli.c
View File

@ -120,7 +120,8 @@ extern uint8_t __config_end;
#include "rx/rx.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"
@ -2114,12 +2115,24 @@ static void cliBeeper(char *cmdline)
}
#endif
#ifdef USE_RX_FRSKY_D
void cliFrSkyBind(char *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
default:
cliPrint("Not supported.");
break;
}
}
static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
{
@ -3643,8 +3656,8 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif
#endif
#ifdef USE_RX_FRSKY_D
CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky RX", NULL, cliFrSkyBind),
#ifdef USE_RX_FRSKY_SPI
CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky SPI RX", NULL, cliFrSkyBind),
#endif
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef USE_GPS

18
src/main/interface/settings.c
View File

@ -65,8 +65,8 @@
#include "io/vtx_rtc6705.h"
#include "rx/rx.h"
#include "rx/cc2500_frsky_common.h"
#include "rx/spektrum.h"
#include "rx/frsky_d.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
@ -195,6 +195,7 @@ static const char * const lookupTableRxSpi[] = {
"H8_3D",
"INAV",
"FRSKY_D",
"FRSKY_X",
"FLYSKY",
"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) },
#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) },
#endif
#endif // USE_TELEMETRY_FRSKY
{ "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) },
#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) },
#endif
#endif
#endif // USE_TELEMETRY
// PG_LED_STRIP_CONFIG
#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) },
#endif
#ifdef USE_RX_FRSKY_D
{ "frsky_d_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, autoBind) },
{ "frsky_d_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindTxId) },
{ "frsky_d_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindOffset) },
{ "frsky_d_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindHopData) },
#ifdef USE_RX_FRSKY_SPI
{ "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, autoBind) },
{ "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindTxId) },
{ "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindOffset) },
{ "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
{ "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

36
src/main/rx/cc2500_frsky_common.h Normal file
View 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
View 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
src/main/rx/frsky_d.h → src/main/rx/cc2500_frsky_d.h
View File

@ -17,23 +17,10 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "rx.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 frSkyDSetRcData(uint16_t *rcData, const 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
View 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
View 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
View 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
View 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();

2
src/main/rx/fport.c
View File

@ -310,7 +310,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
#if defined(USE_TELEMETRY_SMARTPORT)
} else {
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) {
clearToSend = false;
}

738
src/main/rx/frsky_d.c
View File

@ -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

12
src/main/rx/rx_spi.c
View File

@ -35,7 +35,8 @@
#include "rx/rx.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_syma.h"
#include "rx/nrf24_v202.h"
@ -113,13 +114,20 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
break;
#endif
#ifdef USE_RX_FRSKY_D
#ifdef USE_RX_FRSKY_SPI_D
case RX_SPI_FRSKY_D:
protocolInit = frSkyDInit;
protocolDataReceived = frSkyDDataReceived;
protocolSetRcDataFromPayload = frSkyDSetRcData;
break;
#endif
#ifdef USE_RX_FRSKY_SPI_X
case RX_SPI_FRSKY_X:
protocolInit = frSkyXInit;
protocolDataReceived = frSkyXDataReceived;
protocolSetRcDataFromPayload = frSkyXSetRcData;
break;
#endif
#ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A:

1
src/main/rx/rx_spi.h
View File

@ -30,6 +30,7 @@ typedef enum {
RX_SPI_NRF24_H8_3D,
RX_SPI_NRF24_INAV,
RX_SPI_FRSKY_D,
RX_SPI_FRSKY_X,
RX_SPI_A7105_FLYSKY,
RX_SPI_A7105_FLYSKY_2A,
RX_SPI_PROTOCOL_COUNT

32
src/main/target/MIDELICF3/target.h
View File

@ -99,25 +99,33 @@
#define RX_SPI_INSTANCE SPI1
#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 RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_D
#define USE_FRSKY_D_TELEMETRY
#define USE_FRSKY_RX_PA_LNA
#define USE_FRSKY_RX_DIVERSITY
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X
#define USE_RX_FRSKY_SPI_TELEMETRY
#define RX_NSS_PIN SPI1_NSS_PIN
#define RX_SCK_PIN SPI1_SCK_PIN
#define RX_MISO_PIN SPI1_MISO_PIN
#define RX_MOSI_PIN SPI1_MOSI_PIN
#define FRSKY_RX_GDO_0_PIN PB0
#define FRSKY_RX_ANT_SEL_PIN PB2
#define FRSKY_RX_TX_EN_PIN PB1
#define FRSKY_RX_RX_EN_PIN PB11
#define FRSKY_RX_LED_PIN PB6
#define RX_FRSKY_SPI_GDO_0_PIN PB0
#define RX_FRSKY_SPI_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

4
src/main/target/MIDELICF3/target.mk
View File

@ -6,4 +6,6 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/cc2500.c \
rx/frsky_d.c
rx/cc2500_frsky_shared.c \
rx/cc2500_frsky_d.c \
rx/cc2500_frsky_x.c

5
src/main/target/common_fc_post.h
View File

@ -73,3 +73,8 @@
#undef VTX_TRAMP
#undef VTX_SMARTAUDIO
#endif
#if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X)
#define USE_RX_FRSKY_SPI
#endif

1
src/main/telemetry/msp_shared.c
View File

@ -132,6 +132,7 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
sbufAdvance(frameBuf, frameBytesRemaining);
sbufWriteData(rxBuf, payload, frameBytesRemaining);
lastSeq = seqNumber;
return false;
} else {
sbufReadData(frameBuf, payload, bufferBytesRemaining);

102
src/main/telemetry/smartport.c
View File

@ -10,7 +10,7 @@
#include "platform.h"
#ifdef USE_TELEMETRY
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
#include "common/axis.h"
#include "common/color.h"
@ -65,27 +65,6 @@ enum
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
enum
{
@ -166,7 +145,7 @@ static smartPortWriteFrameFn *smartPortWriteFrame;
static bool smartPortMspReplyPending = false;
#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 smartPortRxBytes = 0;
@ -188,7 +167,7 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS
if (awaitingSensorId) {
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
*clearToSend = true;
skipUntilStart = true;
@ -210,6 +189,12 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS
if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
rxBuffer[smartPortRxBytes++] = (uint8_t)c;
checksum += c;
if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
skipUntilStart = true;
return (smartPortPayload_t *)&rxBuffer;
}
} else {
skipUntilStart = true;
@ -386,17 +371,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
static uint8_t t2Cnt = 0;
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 :
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
uint16_t vfasVoltage;
@ -430,28 +404,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break;
//case FSSP_DATAID_ADC1 :
//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_VARIO :
if (sensors(SENSOR_BARO)) {
@ -564,6 +516,35 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
}
break;
#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 :
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)
@ -584,6 +565,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
}
}
static bool serialCheckQueueEmpty(void)
{
return (serialRxBytesWaiting(smartPortSerialPort) == 0);
}
void handleSmartPortTelemetry(void)
{
static bool clearToSend = false;
@ -597,7 +583,7 @@ void handleSmartPortTelemetry(void)
smartPortPayload_t *payload = NULL;
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
uint8_t c = serialRead(smartPortSerialPort);
payload = smartPortDataReceiveSerial(c, &clearToSend);
payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
}
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);

24
src/main/telemetry/smartport.h
View File

@ -13,6 +13,27 @@
#define SMARTPORT_MSP_TX_BUF_SIZE 256
#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 {
uint8_t frameId;
uint16_t valueId;
@ -20,6 +41,7 @@ typedef struct smartPortPayload_s {
} __attribute__((packed)) smartPortPayload_t;
typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
typedef bool smartPortCheckQueueEmptyFn(void);
bool initSmartPortTelemetry(void);
void checkSmartPortTelemetryState(void);
@ -28,6 +50,8 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx
void handleSmartPortTelemetry(void);
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;
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);