Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Merge pull request #4683 from mikeller/added_frsky_x_spi_rx

Browse source 
Added FrSky X SPI RX protocol.
This commit is contained in:
Michael Keller 2017-11-30 18:02:35 +13:00 • committed by GitHub
commit a72a46c604
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
22 changed files with 1708 additions and 848 deletions
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

@ -118,7 +118,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"
@ -2112,12 +2113,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)
{
@ -3641,8 +3654,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);