diff --git a/Makefile b/Makefile index f0a3ba2eb8..61eb107b35 100644 --- a/Makefile +++ b/Makefile @@ -592,6 +592,7 @@ HIGHEND_SRC = \ sensors/barometer.c \ telemetry/telemetry.c \ telemetry/crsf.c \ + telemetry/srxl.c \ telemetry/frsky.c \ telemetry/hott.c \ telemetry/smartport.c \ diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index c79ce8d8bd..b9f1d625b3 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -18,8 +18,9 @@ #include #include #include - +#include "string.h" #include "platform.h" +#include "common/maths.h" #ifdef SERIAL_RX @@ -41,19 +42,20 @@ #include "rx/rx.h" #include "rx/spektrum.h" +#include "config/feature.h" + // driver for spektrum satellite receiver / sbus #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 -#define SPEKTRUM_2048_CHANNEL_COUNT 12 -#define SPEKTRUM_1024_CHANNEL_COUNT 7 +#define SPEKTRUM_2048_CHANNEL_COUNT 12 +#define SPEKTRUM_1024_CHANNEL_COUNT 7 -#define SPEK_FRAME_SIZE 16 -#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 +#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 -#define SPEKTRUM_BAUDRATE 115200 +#define SPEKTRUM_BAUDRATE 115200 -#define SPEKTRUM_MAX_FADE_PER_SEC 40 -#define SPEKTRUM_FADE_REPORTS_PER_SEC 2 +#define SPEKTRUM_MAX_FADE_PER_SEC 40 +#define SPEKTRUM_FADE_REPORTS_PER_SEC 2 static uint8_t spek_chan_shift; static uint8_t spek_chan_mask; @@ -70,6 +72,7 @@ static uint8_t rssi_channel; // Stores the RX RSSI channel. static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static rxRuntimeConfig_t *rxRuntimeConfigPtr; +static serialPort_t *serialPort; #ifdef SPEKTRUM_BIND static IO_t BindPin = DEFIO_IO(NONE); @@ -78,6 +81,10 @@ static IO_t BindPin = DEFIO_IO(NONE); static IO_t BindPlug = DEFIO_IO(NONE); #endif +static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX]; +static uint8_t telemetryBufLen = 0; + +void srxlRxSendTelemetryData(void); // Receive ISR callback static void spektrumDataReceive(uint16_t c) @@ -146,6 +153,9 @@ static uint8_t spektrumFrameStatus(void) } } + if (telemetryBufLen) { + srxlRxSendTelemetryData(); + } return RX_FRAME_COMPLETE; } @@ -275,15 +285,22 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig #ifdef TELEMETRY bool portShared = telemetryCheckRxPortShared(portConfig); + bool srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SPEKTRUM2048); #else + bool srxlEnabled = false; bool portShared = false; #endif - serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + serialPort = openSerialPort(portConfig->identifier, + FUNCTION_RX_SERIAL, + spektrumDataReceive, + SPEKTRUM_BAUDRATE, + portShared || srxlEnabled ? MODE_RXTX : MODE_RX, + SERIAL_NOT_INVERTED | (srxlEnabled ? SERIAL_BIDIR : 0)); #ifdef TELEMETRY if (portShared) { - telemetrySharedPort = spektrumPort; + telemetrySharedPort = serialPort; } #endif @@ -292,7 +309,29 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig rssi_channel = 0; } - return spektrumPort != NULL; + return serialPort != NULL; } + +void srxlRxWriteTelemetryData(const void *data, int len) +{ + len = MIN(len, (int)sizeof(telemetryBuf)); + memcpy(telemetryBuf, data, len); + telemetryBufLen = len; +} + +void srxlRxSendTelemetryData(void) +{ + // if there is telemetry data to write + if (telemetryBufLen > 0) { + serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); + telemetryBufLen = 0; // reset telemetry buffer + } +} + +bool srxlRxIsActive(void) +{ + return serialPort != NULL; +} + #endif // SERIAL_RX diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 44aab64192..58ed4a3cde 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -17,8 +17,15 @@ #pragma once -#define SPEKTRUM_SAT_BIND_DISABLED 0 -#define SPEKTRUM_SAT_BIND_MAX 10 +#define SPEKTRUM_SAT_BIND_DISABLED 0 +#define SPEKTRUM_SAT_BIND_MAX 10 + +#define SPEK_FRAME_SIZE 16 +#define SRXL_FRAME_OVERHEAD 5 +#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD) void spektrumBind(rxConfig_t *rxConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); + +void srxlRxWriteTelemetryData(const void *data, int len); +bool srxlRxIsActive(void); \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index 86d53c9dd1..c2e7889188 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -87,6 +87,7 @@ #define USE_DASHBOARD #define USE_MSP_DISPLAYPORT #define TELEMETRY_CRSF +#define TELEMETRY_SRXL #define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK #define USE_RX_MSP diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c new file mode 100644 index 0000000000..09cdaea941 --- /dev/null +++ b/src/main/telemetry/srxl.c @@ -0,0 +1,270 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef TELEMETRY + +#include "config/feature.h" +#include "build/version.h" + +#include "common/streambuf.h" +#include "common/utils.h" + +#include "sensors/battery.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "io/gps.h" + +#include "flight/imu.h" + +#include "rx/rx.h" +#include "rx/spektrum.h" + +#include "telemetry/telemetry.h" +#include "telemetry/srxl.h" + +#include "fc/config.h" + +#define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz + +#define SRXL_ADDRESS_FIRST 0xA5 +#define SRXL_ADDRESS_SECOND 0x80 +#define SRXL_PACKET_LENGTH 0x15 + +#define SRXL_FRAMETYPE_TELE_QOS 0x7F +#define SRXL_FRAMETYPE_TELE_RPM 0x7E +#define SRXL_FRAMETYPE_POWERBOX 0x0A +#define SRXL_FRAMETYPE_SID 0x00 + +static bool srxlTelemetryEnabled; +static uint16_t srxlCrc; +static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX]; + +#define SRXL_POLY 0x1021 +static uint16_t srxlCrc16(uint16_t crc, uint8_t data) +{ + crc = crc ^ data << 8; + for (int i = 0; i < 8; i++) { + if (crc & 0x8000) { + crc = crc << 1 ^ SRXL_POLY; + } else { + crc = crc << 1; + } + } + return crc; +} + +static void srxlInitializeFrame(sbuf_t *dst) +{ + srxlCrc = 0; + dst->ptr = srxlFrame; + dst->end = ARRAYEND(srxlFrame); + + sbufWriteU8(dst, SRXL_ADDRESS_FIRST); + sbufWriteU8(dst, SRXL_ADDRESS_SECOND); + sbufWriteU8(dst, SRXL_PACKET_LENGTH); +} + +static void srxlSerialize8(sbuf_t *dst, uint8_t v) +{ + sbufWriteU8(dst, v); + srxlCrc = srxlCrc16(srxlCrc, v); +} + +static void srxlSerialize16(sbuf_t *dst, uint16_t v) +{ + // Use BigEndian format + srxlSerialize8(dst, (v >> 8)); + srxlSerialize8(dst, (uint8_t)v); +} + +static void srxlFinalize(sbuf_t *dst) +{ + sbufWriteU16(dst, srxlCrc); + sbufSwitchToReader(dst, srxlFrame); + // write the telemetry frame to the receiver. + srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); +} + +/* +SRXL frame has the structure: +<0xA5><0x80><16-byte telemetry packet><2 Byte CRC of payload> +The shall be 0x15 (length of the 16-byte telemetry packet + overhead). +*/ + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x7F + UINT8 sID; // Secondary ID + UINT16 A; + UINT16 B; + UINT16 L; + UINT16 R; + UINT16 F; + UINT16 H; + UINT16 rxVoltage; // Volts, 0.01V increments +} STRU_TELE_QOS; +*/ +void srxlFrameQos(sbuf_t *dst) +{ + srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_QOS); + srxlSerialize8(dst, SRXL_FRAMETYPE_SID); + srxlSerialize16(dst, 0xFFFF); // A + srxlSerialize16(dst, 0xFFFF); // B + srxlSerialize16(dst, 0xFFFF); // L + srxlSerialize16(dst, 0xFFFF); // R + srxlSerialize16(dst, 0xFFFF); // F + srxlSerialize16(dst, 0xFFFF); // H + srxlSerialize16(dst, 0xFFFF); // rxVoltage +} + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x7E + UINT8 sID; // Secondary ID + UINT16 microseconds; // microseconds between pulse leading edges + UINT16 volts; // 0.01V increments + INT16 temperature; // degrees F + INT8 dBm_A, // Average signal for A antenna in dBm + dBm_B; // Average signal for B antenna in dBm. + // If only 1 antenna, set B = A +} STRU_TELE_RPM; +*/ +void srxlFrameRpm(sbuf_t *dst) +{ + srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM); + srxlSerialize8(dst, SRXL_FRAMETYPE_SID); + srxlSerialize16(dst, 0xFFFF); // pulse leading edges + srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V + srxlSerialize16(dst, 0x7FFF); // temperature + srxlSerialize8(dst, 0xFF); // dbmA + srxlSerialize8(dst, 0xFF); // dbmB + + /* unused */ + srxlSerialize16(dst, 0xFFFF); + srxlSerialize16(dst, 0xFFFF); + srxlSerialize16(dst, 0xFFFF); +} + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x0A + UINT8 sID; // Secondary ID + UINT16 volt1; // Volts, 0.01v + UINT16 volt2; // Volts, 0.01v + UINT16 capacity1; // mAh, 1mAh + UINT16 capacity2; // mAh, 1mAh + UINT16 spare16_1; + UINT16 spare16_2; + UINT8 spare; + UINT8 alarms; // Alarm bitmask (see below) +} STRU_TELE_POWERBOX; +*/ +#define TELE_PBOX_ALARM_VOLTAGE_1 (0x01) +#define TELE_PBOX_ALARM_VOLTAGE_2 (0x02) +#define TELE_PBOX_ALARM_CAPACITY_1 (0x04) +#define TELE_PBOX_ALARM_CAPACITY_2 (0x08) +//#define TELE_PBOX_ALARM_RPM (0x10) +//#define TELE_PBOX_ALARM_TEMPERATURE (0x20) +#define TELE_PBOX_ALARM_RESERVED_1 (0x40) +#define TELE_PBOX_ALARM_RESERVED_2 (0x80) + +void srxlFramePowerBox(sbuf_t *dst) +{ + srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX); + srxlSerialize8(dst, SRXL_FRAMETYPE_SID); + srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat1 + srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat2 + srxlSerialize16(dst, amperage / 10); + srxlSerialize16(dst, 0xFFFF); + + srxlSerialize16(dst, 0xFFFF); // spare + srxlSerialize16(dst, 0xFFFF); // spare + srxlSerialize8(dst, 0xFF); // spare + srxlSerialize8(dst, 0x00); // ALARMS +} + +// schedule array to decide how often each type of frame is sent +#define SRXL_SCHEDULE_COUNT_MAX 3 + +typedef void (*srxlSchedulePtr)(sbuf_t *dst); +const srxlSchedulePtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = { + /* must send srxlFrameQos, Rpm and then alternating items of our own */ + srxlFrameQos, + srxlFrameRpm, + srxlFramePowerBox +}; + +static void processSrxl(void) +{ + static uint8_t srxlScheduleIndex = 0; + + sbuf_t srxlPayloadBuf; + sbuf_t *dst = &srxlPayloadBuf; + + srxlSchedulePtr srxlPtr = srxlScheduleFuncs[srxlScheduleIndex]; + if (srxlPtr) { + srxlInitializeFrame(dst); + srxlPtr(dst); + srxlFinalize(dst); + } + srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX; +} + +void initSrxlTelemetry(void) +{ + // check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX) + // and feature is enabled, if so, set SRXL telemetry enabled + srxlTelemetryEnabled = srxlRxIsActive(); + } + +bool checkSrxlTelemetryState(void) +{ + return srxlTelemetryEnabled; +} + +/* + * Called periodically by the scheduler + */ +void handleSrxlTelemetry(timeUs_t currentTimeUs) +{ + static uint32_t srxlLastCycleTime; + + if (!srxlTelemetryEnabled) { + return; + } + + // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz + if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) { + srxlLastCycleTime = currentTimeUs; + processSrxl(); + } +} +#endif diff --git a/src/main/telemetry/srxl.h b/src/main/telemetry/srxl.h new file mode 100644 index 0000000000..33fc329e69 --- /dev/null +++ b/src/main/telemetry/srxl.h @@ -0,0 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "common/time.h" + +void initSrxlTelemetry(void); +bool checkSrxlTelemetryState(void); +void handleSrxlTelemetry(timeUs_t currentTimeUs); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 2579187fc1..fc7e3bc9de 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -47,6 +47,7 @@ #include "telemetry/jetiexbus.h" #include "telemetry/mavlink.h" #include "telemetry/crsf.h" +#include "telemetry/srxl.h" static telemetryConfig_t *telemetryConfig; @@ -78,6 +79,9 @@ void telemetryInit(void) #ifdef TELEMETRY_CRSF initCrsfTelemetry(); #endif +#ifdef TELEMETRY_SRXL + initSrxlTelemetry(); +#endif telemetryCheckState(); } @@ -126,6 +130,9 @@ void telemetryCheckState(void) #ifdef TELEMETRY_CRSF checkCrsfTelemetryState(); #endif +#ifdef TELEMETRY_SRXL + checkSrxlTelemetryState(); +#endif } void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -156,6 +163,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb #ifdef TELEMETRY_CRSF handleCrsfTelemetry(currentTime); #endif +#ifdef TELEMETRY_SRXL + handleSrxlTelemetry(currentTime); +#endif } #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)