Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Add FPort support

Browse source 
FPort code is mostly imported from Betaflight. To make future RX
updates easier, a few more changes have been done to make the
RX/Telemetry code in BF and INAV a bit closer.

- Imported RSSI handling code from BF with some changes
- frsky_vfas_cell_voltage setting renamed to report_cell_voltage
- Added telemetry_halfduplex setting
- sbus_inversion renamed to serialrx_inverted, should apply to all
protocols eventually
- New MSP cmds MSP_TX_INFO/MSP_SET_TX_INFO now allow setting the RSSI
via MSP, thus RX_MSP doesn't need to use an extra channel anymore.
This commit is contained in:
Alberto García Hierro 2018-03-13 12:43:17 +00:00
parent 5f2e976fd1
commit a9c91b6e07
50 changed files with 1514 additions and 657 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -631,6 +631,7 @@ COMMON_SRC = \
io/rcdevice.c \
io/rcdevice_cam.c \
msp/msp_serial.c \
rx/fport.c \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
@ -645,6 +646,7 @@ COMMON_SRC = \
rx/rx_spi.c \
rx/crsf.c \
rx/sbus.c \
rx/sbus_channels.c \
rx/spektrum.c \
rx/sumd.c \
rx/sumh.c \
@ -717,6 +719,7 @@ HIGHEND_SRC = \
telemetry/ibus.c \
telemetry/ltm.c \
telemetry/mavlink.c \
telemetry/msp_shared.c \
telemetry/smartport.c \
telemetry/telemetry.c \
io/vtx_string.c \

2
src/main/blackbox/blackbox.c
View file

@ -1174,7 +1174,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
#endif
blackboxCurrent->rssi = rssi;
blackboxCurrent->rssi = getRSSI();
#ifdef USE_SERVOS
//Tail servo for tricopters

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

@ -57,6 +57,8 @@ typedef enum {
DEBUG_PITOT,
DEBUG_AGL,
DEBUG_FLOW_RAW,
DEBUG_SBUS,
DEBUG_FPORT,
DEBUG_ALWAYS,
DEBUG_COUNT
} debugType_e;

7
src/main/common/streambuf.c
View file

@ -20,6 +20,13 @@
#include "streambuf.h"
sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
{
sbuf->ptr = ptr;
sbuf->end = end;
return sbuf;
}
void sbufWriteU8(sbuf_t *dst, uint8_t val)
{
*dst->ptr++ = val;

2
src/main/common/streambuf.h
View file

@ -28,6 +28,8 @@ typedef struct sbuf_s {
uint8_t *end;
} sbuf_t;
sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end);
void sbufWriteU8(sbuf_t *dst, uint8_t val);
void sbufWriteU16(sbuf_t *dst, uint16_t val);
void sbufWriteU32(sbuf_t *dst, uint32_t val);

21
src/main/fc/fc_msp.c
View file

@ -105,7 +105,6 @@
#endif
extern timeDelta_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c
static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@ -538,7 +537,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_ANALOG:
sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage() / 10, 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, rssi);
sbufWriteU16(dst, getRSSI());
sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break;
@ -549,7 +548,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, constrain(getPower(), 0, 0x7FFFFFFF)); // power draw
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, (uint16_t)constrain(getMWhDrawn(), 0, 0xFFFF)); // milliWatt hours drawn from battery
sbufWriteU16(dst, rssi);
sbufWriteU16(dst, getRSSI());
sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2));
sbufWriteU32(dst, getBatteryRemainingCapacity());
@ -1229,6 +1228,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
break;
case MSP_TX_INFO:
sbufWriteU8(dst, rssiSource);
uint8_t rtcDateTimeIsSet = 0;
dateTime_t dt;
if (rtcGetDateTime(&dt)) {
rtcDateTimeIsSet = 1;
}
sbufWriteU8(dst, rtcDateTimeIsSet);
break;
case MSP_RTC:
{
int32_t seconds = 0;
@ -2244,6 +2253,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
case MSP_SET_TX_INFO:
{
setRssiMsp(sbufReadU8(src));
}
break;
case MSP_SET_NAME:
{
char *name = systemConfigMutable()->name;

11
src/main/fc/settings.yaml
View file

@ -25,7 +25,7 @@ tables:
values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF"]
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT"]
- name: rx_spi_protocol
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
enum: rx_spi_protocol_e
@ -69,7 +69,7 @@ tables:
- name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW","ALWAYS"]
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -327,7 +327,7 @@ groups:
- name: serialrx_provider
condition: USE_SERIAL_RX
table: serial_rx
- name: sbus_inversion
- name: serialrx_inverted
condition: USE_SERIAL_RX
type: bool
- name: rx_spi_protocol
@ -1318,7 +1318,7 @@ groups:
- name: frsky_vfas_precision
min: FRSKY_VFAS_PRECISION_LOW
max: FRSKY_VFAS_PRECISION_HIGH
- name: frsky_vfas_cell_voltage
- name: report_cell_voltage
type: bool
- name: hott_alarm_sound_interval
field: hottAlarmSoundInterval
@ -1341,6 +1341,9 @@ groups:
field: ltmUpdateRate
condition: USE_TELEMETRY_LTM
table: ltm_rates
- name: telemetry_halfduplex
field: halfDuplex
type: bool
- name: PG_ELERES_CONFIG
type: eleresConfig_t

6
src/main/io/ledstrip.c
View file

@ -75,8 +75,6 @@
#include "telemetry/telemetry.h"
extern uint16_t rssi; // FIXME dependency on mw.c
PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0);
static bool ledStripInitialised = false;
@ -488,7 +486,7 @@ static void applyLedFixedLayers(void)
case LED_FUNCTION_RSSI:
color = HSV(RED);
hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);
hOffset += scaleRange(getRSSI() * 100, 0, 1023, -30, 120);
break;
default:
@ -615,7 +613,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
int timeOffset = 1;
if (updateNow) {
state = (rssi * 100) / 1023;
state = (getRSSI() * 100) / 1023;
if (state > 50) {
flash = false;

2
src/main/io/osd.c
View file

@ -436,7 +436,7 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
*/
static uint16_t osdConvertRSSI(void)
{
uint16_t osdRssi = rssi * 100 / 1024; // change range
uint16_t osdRssi = getRSSI() * 100 / 1024; // change range
if (osdRssi >= 100) {
osdRssi = 99;
}

3
src/main/msp/msp_protocol.h
View file

@ -242,6 +242,9 @@
#define MSP_DISPLAYPORT 182
#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
//
// Multwii original MSP commands
//

4
src/main/rx/crsf.c
View file

@ -162,8 +162,10 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
return crc;
}
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (crsfFrameDone) {
crsfFrameDone = false;
if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {

398
src/main/rx/fport.c Normal file
View file

@ -0,0 +1,398 @@
/*
* 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"
#if defined(USE_SERIAL_RX)
#include "build/debug.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "io/serial.h"
#ifdef USE_TELEMETRY
#include "telemetry/telemetry.h"
#include "telemetry/smartport.h"
#endif
#include "rx/rx.h"
#include "rx/sbus_channels.h"
#include "rx/fport.h"
#define FPORT_TIME_NEEDED_PER_FRAME_US 3000
#define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
#define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define FPORT_MAX_TELEMETRY_AGE_MS 500
#define FPORT_FRAME_MARKER 0x7E
#define FPORT_ESCAPE_CHAR 0x7D
#define FPORT_ESCAPE_MASK 0x20
#define FPORT_CRC_VALUE 0xFF
#define FPORT_BAUDRATE 115200
#define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
enum {
DEBUG_FPORT_FRAME_INTERVAL = 0,
DEBUG_FPORT_FRAME_ERRORS,
DEBUG_FPORT_FRAME_LAST_ERROR,
DEBUG_FPORT_TELEMETRY_DELAY,
};
enum {
DEBUG_FPORT_NO_ERROR = 0,
DEBUG_FPORT_ERROR_TIMEOUT,
DEBUG_FPORT_ERROR_OVERSIZE,
DEBUG_FPORT_ERROR_SIZE,
DEBUG_FPORT_ERROR_CHECKSUM,
DEBUG_FPORT_ERROR_TYPE,
DEBUG_FPORT_ERROR_TYPE_SIZE,
};
enum {
FPORT_FRAME_TYPE_CONTROL = 0x00,
FPORT_FRAME_TYPE_TELEMETRY_REQUEST = 0x01,
FPORT_FRAME_TYPE_TELEMETRY_RESPONSE = 0x81,
};
enum {
FPORT_FRAME_ID_NULL = 0x00, // (master/slave)
FPORT_FRAME_ID_DATA = 0x10, // (master/slave)
FPORT_FRAME_ID_READ = 0x30, // (master)
FPORT_FRAME_ID_WRITE = 0x31, // (master)
FPORT_FRAME_ID_RESPONSE = 0x32, // (slave)
};
typedef struct fportControlData_s {
sbusChannels_t channels;
uint8_t rssi;
} fportControlData_t;
typedef union fportData_s {
fportControlData_t controlData;
smartPortPayload_t telemetryData;
} fportData_t;
typedef struct fportFrame_s {
uint8_t type;
fportData_t data;
} fportFrame_t;
#if defined(USE_SERIALRX_FPORT)
static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
#define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
#define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
#define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
#define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
#define NUM_RX_BUFFERS 3
#define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
typedef struct fportBuffer_s {
uint8_t data[BUFFER_SIZE];
uint8_t length;
} fportBuffer_t;
static fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
static volatile uint8_t rxBufferWriteIndex = 0;
static volatile uint8_t rxBufferReadIndex = 0;
static volatile timeUs_t lastTelemetryFrameReceivedUs;
static volatile bool clearToSend = false;
static volatile uint8_t framePosition = 0;
static smartPortPayload_t *mspPayload = NULL;
static timeUs_t lastRcFrameReceivedMs = 0;
static serialPort_t *fportPort;
static bool telemetryEnabled = false;
static void reportFrameError(uint8_t errorReason) {
static volatile uint16_t frameErrors = 0;
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors);
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason);
}
// Receive ISR callback
static void fportDataReceive(uint16_t c, void *data)
{
UNUSED(data);
static timeUs_t frameStartAt = 0;
static bool escapedCharacter = false;
static timeUs_t lastFrameReceivedUs = 0;
static bool telemetryFrame = false;
const timeUs_t currentTimeUs = micros();
clearToSend = false;
if (framePosition > 1 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
framePosition = 0;
}
uint8_t val = (uint8_t)c;
if (val == FPORT_FRAME_MARKER) {
if (framePosition > 1) {
const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
if (nextWriteIndex != rxBufferReadIndex) {
rxBuffer[rxBufferWriteIndex].length = framePosition - 1;
rxBufferWriteIndex = nextWriteIndex;
}
if (telemetryFrame) {
clearToSend = true;
lastTelemetryFrameReceivedUs = currentTimeUs;
telemetryFrame = false;
}
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
lastFrameReceivedUs = currentTimeUs;
escapedCharacter = false;
}
frameStartAt = currentTimeUs;
framePosition = 1;
} else if (framePosition > 0) {
if (framePosition >= BUFFER_SIZE + 1) {
framePosition = 0;
reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
} else {
if (escapedCharacter) {
val = val ^ FPORT_ESCAPE_MASK;
escapedCharacter = false;
} else if (val == FPORT_ESCAPE_CHAR) {
escapedCharacter = true;
return;
}
if (framePosition == 2 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
telemetryFrame = true;
}
rxBuffer[rxBufferWriteIndex].data[framePosition - 1] = val;
framePosition = framePosition + 1;
}
}
}
#if defined(USE_TELEMETRY_SMARTPORT)
static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
{
framePosition = 0;
uint16_t checksum = 0;
smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort);
smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE, &checksum, fportPort);
smartPortWriteFrameSerial(payload, fportPort, checksum);
}
#endif
static bool checkChecksum(uint8_t *data, uint8_t length)
{
uint16_t checksum = 0;
for (unsigned i = 0; i < length; i++) {
checksum = checksum + *(uint8_t *)(data + i);
}
checksum = (checksum & 0xff) + (checksum >> 8);
return checksum == FPORT_CRC_VALUE;
}
static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
static smartPortPayload_t payloadBuffer;
static bool hasTelemetryRequest = false;
uint8_t result = RX_FRAME_PENDING;
if (rxBufferReadIndex != rxBufferWriteIndex) {
uint8_t bufferLength = rxBuffer[rxBufferReadIndex].length;
uint8_t frameLength = rxBuffer[rxBufferReadIndex].data[0];
if (frameLength != bufferLength - 2) {
reportFrameError(DEBUG_FPORT_ERROR_SIZE);
} else {
if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
} else {
fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
switch (frame->type) {
case FPORT_FRAME_TYPE_CONTROL:
if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
} else {
result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
lastRcFrameReceivedMs = millis();
}
break;
case FPORT_FRAME_TYPE_TELEMETRY_REQUEST:
if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST) {
reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
} else {
#if defined(USE_TELEMETRY_SMARTPORT)
if (!telemetryEnabled) {
break;
}
switch(frame->data.telemetryData.frameId) {
case FPORT_FRAME_ID_NULL:
case FPORT_FRAME_ID_DATA: // never used
hasTelemetryRequest = true;
break;
case FPORT_FRAME_ID_READ:
case FPORT_FRAME_ID_WRITE: // never used
memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
mspPayload = &payloadBuffer;
break;
default:
break;
}
#endif
}
break;
default:
reportFrameError(DEBUG_FPORT_ERROR_TYPE);
break;
}
}
}
rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
}
if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
hasTelemetryRequest = false;
result = result | RX_FRAME_PROCESSING_REQUIRED;
}
if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
lastRcFrameReceivedMs = 0;
}
return result;
}
static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
#if defined(USE_TELEMETRY_SMARTPORT)
timeUs_t currentTimeUs = micros();
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
clearToSend = false;
}
if (clearToSend) {
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
if (clearToSend) {
smartPortWriteFrameFport(&emptySmartPortFrame);
clearToSend = false;
}
}
mspPayload = NULL;
#endif
return true;
}
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
rxRuntimeConfig->channelData = sbusChannelData;
sbusChannelsInit(rxConfig, rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
fportPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL,
fportDataReceive,
NULL,
FPORT_BAUDRATE,
MODE_RXTX,
FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
);
if (fportPort) {
#if defined(USE_TELEMETRY_SMARTPORT)
telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
#endif
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
}
}
return fportPort != NULL;
}
#endif
#endif

20
src/main/rx/fport.h Normal file
View file

@ -0,0 +1,20 @@
/*
* 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
bool fportRxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

4
src/main/rx/ibus.c
View file

@ -171,8 +171,10 @@ static void updateChannelData(void) {
}
}
static uint8_t ibusFrameStatus(void)
static uint8_t ibusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
uint8_t frameStatus = RX_FRAME_PENDING;
if (!ibusFrameDone) {

4
src/main/rx/jetiexbus.c
View file

@ -372,8 +372,10 @@ static void jetiExBusDataReceive(uint16_t c, void *rxCallbackData)
// Check if it is time to read a frame from the data...
uint8_t jetiExBusFrameStatus(void)
static uint8_t jetiExBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (jetiExBusFrameState != EXBUS_STATE_RECEIVED)
return RX_FRAME_PENDING;

2
src/main/rx/jetiexbus.h
View file

@ -18,5 +18,3 @@
#pragma once
bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
uint8_t jetiExBusFrameStatus(void);

4
src/main/rx/msp.c
View file

@ -52,8 +52,10 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
rxMspFrameDone = true;
}
uint8_t rxMspFrameStatus(void)
static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!rxMspFrameDone) {
return RX_FRAME_PENDING;
}

1
src/main/rx/msp.h
View file

@ -19,6 +19,5 @@
#include "rx/rx.h"
uint8_t rxMspFrameStatus(void);
void rxMspFrameReceive(uint16_t *frame, int channelCount);
void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

8
src/main/rx/pwm.c
View file

@ -54,8 +54,10 @@ static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan
return channelData[channel];
}
static uint8_t pwmFrameStatus(void)
static uint8_t pwmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
timeUs_t currentTimeUs = micros();
// PWM doesn't indicate individual updates, if low level code indicates that we have valid signal
@ -76,8 +78,10 @@ static uint8_t pwmFrameStatus(void)
return RX_FRAME_PENDING;
}
static uint8_t ppmFrameStatus(void)
static uint8_t ppmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
// PPM receiver counts received frames so we actually know if new data is available
if (isPPMDataBeingReceived()) {
for (int channel = 0; channel < MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; channel++) {

184
src/main/rx/rx.c
View file

@ -49,6 +49,7 @@
#include "io/serial.h"
#include "rx/rx.h"
#include "rx/fport.h"
#include "rx/pwm.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
@ -68,13 +69,25 @@
const char rcChannelLetters[] = "AERT5678";
uint16_t rssi = 0; // range: [0;1023]
static uint16_t rssi = 0; // range: [0;1023]
static timeUs_t lastMspRssiUpdateUs = 0;
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
rssiSource_e rssiSource;
static bool rxDataProcessingRequired = false;
static bool auxiliaryProcessingRequired = false;
static bool rxSignalReceived = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
static timeUs_t rxLastUpdateTimeUs = 0;
static timeUs_t rxLastValidFrameTimeUs = 0;
static timeUs_t rxNextUpdateAtUs = 0;
static uint32_t needRxSignalBefore = 0;
static uint32_t needRxSignalMaxDelayUs;
static uint32_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0;
int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
@ -110,7 +123,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.serialrx_provider = SERIALRX_PROVIDER,
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
.spektrum_sat_bind = 0,
.sbus_inversion = 1,
.serialrx_inverted = 0,
.midrc = RX_MIDRC,
.mincheck = 1100,
.maxcheck = 1900,
@ -150,8 +163,10 @@ static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
return PPM_RCVR_TIMEOUT;
}
static uint8_t nullFrameStatus(void)
static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
return RX_FRAME_PENDING;
}
@ -207,6 +222,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
case SERIALRX_CRSF:
enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_FPORT
case SERIALRX_FPORT:
enabled = fportRxInit(rxConfig, rxRuntimeConfig);
break;
#endif
default:
enabled = false;
@ -291,6 +311,10 @@ void rxInit(void)
// Already configured for NONE
break;
}
if (rxConfig()->rssi_channel > 0) {
rssiSource = RSSI_SOURCE_RX_CHANNEL;
}
}
static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
@ -314,10 +338,14 @@ bool rxAreFlightChannelsValid(void)
void suspendRxSignal(void)
{
failsafeOnRxSuspend();
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
}
void resumeRxSignal(void)
{
suspendRxSignalUntil = micros();
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxResume();
}
@ -325,21 +353,49 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
{
UNUSED(currentDeltaTime);
bool rxDataReceived = false;
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxSignalReceived = ((frameStatus & RX_FRAME_FAILSAFE) == 0);
rxLastValidFrameTimeUs = currentTimeUs;
}
else { // RX_FRAME_PENDING
// Check for valid signal timeout - if we are RX_FRAME_PENDING for too long assume signall loss
if ((currentTimeUs - rxLastValidFrameTimeUs) >= rxRuntimeConfig.rxSignalTimeout) {
if (rxSignalReceived) {
if (currentTimeUs >= needRxSignalBefore) {
rxSignalReceived = false;
}
}
return rxDataReceived || ((int32_t)(currentTimeUs - rxLastUpdateTimeUs) >= 0); // data driven or 50Hz
#if defined(USE_PWM) || defined(USE_PPM)
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxDataProcessingRequired = true;
rxSignalReceived = true;
rxIsInFailsafeMode = false;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
resetPPMDataReceivedState();
}
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
if (isPWMDataBeingReceived()) {
rxDataProcessingRequired = true;
rxSignalReceived = true;
rxIsInFailsafeMode = false;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
} else
#endif
{
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataProcessingRequired = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
auxiliaryProcessingRequired = true;
}
}
if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
rxDataProcessingRequired = true;
}
return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
}
#define FILTERING_SAMPLE_COUNT 5
@ -377,8 +433,29 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
const timeMs_t currentTimeMs = millis();
rxLastUpdateTimeUs = currentTimeUs + DELAY_50_HZ;
if (auxiliaryProcessingRequired) {
auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
}
if (!rxDataProcessingRequired) {
return false;
}
rxDataProcessingRequired = false;
rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
// only proceed when no more samples to skip and suspend period is over
if (skipRxSamples) {
if (currentTimeUs > suspendRxSignalUntil) {
skipRxSamples--;
}
return true;
}
rxFlightChannelsValid = true;
@ -435,6 +512,50 @@ void parseRcChannels(const char *input)
}
}
void setRssiFiltered(uint16_t newRssi, rssiSource_e source)
{
if (source != rssiSource) {
return;
}
rssi = newRssi;
}
#define RSSI_SAMPLE_COUNT 16
#define RSSI_MAX_VALUE 1023
void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source)
{
if (source != rssiSource) {
return;
}
static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
static uint8_t rssiSampleIndex = 0;
static unsigned sum = 0;
sum = sum + rssiValue;
sum = sum - rssiSamples[rssiSampleIndex];
rssiSamples[rssiSampleIndex] = rssiValue;
rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
rssi = rssiMean;
}
void setRssiMsp(uint8_t newMspRssi)
{
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_MSP;
}
if (rssiSource == RSSI_SOURCE_MSP) {
rssi = ((uint16_t)newMspRssi) << 2;
lastMspRssiUpdateUs = micros();
}
}
#ifdef USE_RX_ELERES
static bool updateRSSIeleres(uint32_t currentTime)
{
@ -491,14 +612,25 @@ void updateRSSI(timeUs_t currentTimeUs)
bool rssiUpdated = false;
// Read RSSI
if (rxConfig()->rssi_channel > 0) {
rssiUpdated = updateRSSIPWM();
} else if (feature(FEATURE_RSSI_ADC)) {
rssiUpdated = updateRSSIADC(currentTimeUs);
switch (rssiSource) {
case RSSI_SOURCE_RX_CHANNEL:
updateRSSIPWM();
break;
case RSSI_SOURCE_ADC:
updateRSSIADC(currentTimeUs);
break;
case RSSI_SOURCE_MSP:
if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
rssi = 0;
}
break;
default:
#ifdef USE_RX_ELERES
} else if (rxConfig()->receiverType == RX_TYPE_SPI && rxConfig()->rx_spi_protocol == RFM22_ELERES) {
if (rxConfig()->receiverType == RX_TYPE_SPI && rxConfig()->rx_spi_protocol == RFM22_ELERES) {
rssiUpdated = updateRSSIeleres(currentTimeUs);
}
#endif
break;
}
if (rssiUpdated) {
@ -508,13 +640,15 @@ void updateRSSI(timeUs_t currentTimeUs)
}
// Apply scaling
debug[0] = rssi;
debug[1] = rxConfig()->rssi_scale;
rssi = constrain((uint32_t)rssi * rxConfig()->rssi_scale / 100, 0, 1023);
debug[2] = rssi;
}
}
uint16_t getRSSI(void)
{
return rssi;
}
uint16_t rxGetRefreshRate(void)
{
return rxRuntimeConfig.rxRefreshRate;

34
src/main/rx/rx.h
View file

@ -51,7 +51,8 @@
typedef enum {
RX_FRAME_PENDING = 0, // No new data available from receiver
RX_FRAME_COMPLETE = (1 << 0), // There is new data available
RX_FRAME_FAILSAFE = (1 << 1) // Receiver detected loss of RC link. Only valid when RX_FRAME_COMPLETE is set as well
RX_FRAME_FAILSAFE = (1 << 1), // Receiver detected loss of RC link. Only valid when RX_FRAME_COMPLETE is set as well
RX_FRAME_PROCESSING_REQUIRED = (1 << 2),
} rxFrameState_e;
typedef enum {
@ -74,7 +75,8 @@ typedef enum {
SERIALRX_XBUS_MODE_B_RJ01 = 6,
SERIALRX_IBUS = 7,
SERIALRX_JETIEXBUS = 8,
SERIALRX_CRSF = 9
SERIALRX_CRSF = 9,
SERIALRX_FPORT = 10,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
@ -110,7 +112,7 @@ typedef struct rxConfig_s {
uint8_t receiverType; // RC receiver type (rxReceiverType_e enum)
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
uint8_t serialrx_provider; // Type of UART-based receiver (rxSerialReceiverType_e enum). Only used if receiverType is RX_TYPE_SERIAL
uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers.
uint8_t serialrx_inverted; // Flip the default inversion of the protocol - e.g. sbus (Futaba, FrSKY) is inverted if this is false, uninverted if it's true. Support for uninverted OpenLRS (and modified FrSKY) receivers.
uint8_t halfDuplex; // allow rx to operate in half duplex mode on F4, ignored for F1 and F3.
uint8_t rx_spi_protocol; // type of SPI receiver protocol (rx_spi_protocol_e enum). Only used if receiverType is RX_TYPE_SPI
uint32_t rx_spi_id;
@ -132,9 +134,10 @@ PG_DECLARE(rxConfig_t, rxConfig);
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
struct rxRuntimeConfig_s;
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(void);
typedef struct rxRuntimeConfig_s rxRuntimeConfig_t;
typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig);
typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
typedef struct rxRuntimeConfig_s {
uint8_t channelCount; // number of rc channels as reported by current input driver
@ -143,10 +146,22 @@ typedef struct rxRuntimeConfig_s {
bool requireFiltering;
rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn;
rcProcessFrameFnPtr rcProcessFrameFn;
uint16_t *channelData;
void *frameData;
} rxRuntimeConfig_t;
typedef enum {
RSSI_SOURCE_NONE = 0,
RSSI_SOURCE_ADC,
RSSI_SOURCE_RX_CHANNEL,
RSSI_SOURCE_RX_PROTOCOL,
RSSI_SOURCE_MSP,
} rssiSource_e;
extern rssiSource_e rssiSource;
extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
extern uint16_t rssi;
void rxInit(void);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
@ -156,7 +171,12 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
void parseRcChannels(const char *input);
void setRssiFiltered(uint16_t newRssi, rssiSource_e source);
void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source);
void setRssiMsp(uint8_t newMspRssi);
void updateRSSI(timeUs_t currentTimeUs);
uint16_t getRSSI(void);
void resetAllRxChannelRangeConfigurations(void);
void suspendRxSignal(void);

191
src/main/rx/sbus.c
View file

@ -21,19 +21,23 @@
#include "platform.h"
#ifdef USE_SERIALRX_SBUS
#ifdef USE_SERIAL_RX
#include "build/debug.h"
#include "common/utils.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "io/serial.h"
#ifdef USE_TELEMETRY
#include "telemetry/telemetry.h"
#endif
#include "rx/rx.h"
#include "rx/sbus.h"
#include "telemetry/telemetry.h"
#include "rx/sbus_channels.h"
/*
* Observations
@ -49,58 +53,32 @@
#define SBUS_TIME_NEEDED_PER_FRAME 3000
#ifndef CJMCU
//#define DEBUG_SBUS_PACKETS
#endif
#ifdef DEBUG_SBUS_PACKETS
static uint16_t sbusStateFlags = 0;
#define SBUS_STATE_FAILSAFE (1 << 0)
#define SBUS_STATE_SIGNALLOSS (1 << 1)
#endif
#define SBUS_MAX_CHANNEL 18
#define SBUS_FRAME_SIZE 25
#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
#define SBUS_FRAME_BEGIN_BYTE 0x0F
#define SBUS_BAUDRATE 100000
#if !defined(SBUS_PORT_OPTIONS)
#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
#endif
#define SBUS_DIGITAL_CHANNEL_MIN 173
#define SBUS_DIGITAL_CHANNEL_MAX 1812
static bool sbusFrameDone = false;
enum {
DEBUG_SBUS_FRAME_FLAGS = 0,
DEBUG_SBUS_STATE_FLAGS,
DEBUG_SBUS_FRAME_TIME,
};
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
#define SBUS_FLAG_CHANNEL_17 (1 << 0)
#define SBUS_FLAG_CHANNEL_18 (1 << 1)
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
struct sbusFrame_s {
uint8_t syncByte;
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
unsigned int chan12 : 11;
unsigned int chan13 : 11;
unsigned int chan14 : 11;
unsigned int chan15 : 11;
uint8_t flags;
sbusChannels_t channels;
/**
* The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
*
@ -111,130 +89,87 @@ struct sbusFrame_s {
uint8_t endByte;
} __attribute__ ((__packed__));
typedef union {
typedef union sbusFrame_u {
uint8_t bytes[SBUS_FRAME_SIZE];
struct sbusFrame_s frame;
} sbusFrame_t;
static sbusFrame_t sbusFrame;
typedef struct sbusFrameData_s {
sbusFrame_t frame;
uint32_t startAtUs;
uint16_t stateFlags;
uint8_t position;
bool done;
} sbusFrameData_t;
// Receive ISR callback
static void sbusDataReceive(uint16_t c, void *rxCallbackData)
static void sbusDataReceive(uint16_t c, void *data)
{
UNUSED(rxCallbackData);
sbusFrameData_t *sbusFrameData = data;
static uint8_t sbusFramePosition = 0;
static uint32_t sbusFrameStartAt = 0;
timeUs_t now = micros();
const uint32_t nowUs = micros();
timeDelta_t sbusFrameTime = cmpTimeUs(now, sbusFrameStartAt);
const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
sbusFramePosition = 0;
sbusFrameData->position = 0;
}
if (sbusFramePosition == 0) {
if (sbusFrameData->position == 0) {
if (c != SBUS_FRAME_BEGIN_BYTE) {
return;
}
sbusFrameStartAt = now;
sbusFrameData->startAtUs = nowUs;
}
if (sbusFramePosition < SBUS_FRAME_SIZE) {
sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c;
if (sbusFramePosition == SBUS_FRAME_SIZE) {
// endByte currently ignored
sbusFrameDone = true;
#ifdef DEBUG_SBUS_PACKETS
debug[2] = sbusFrameTime;
#endif
if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrameData->done = false;
} else {
sbusFrameDone = false;
sbusFrameData->done = true;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
}
}
}
uint8_t sbusFrameStatus(void)
static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
if (!sbusFrameDone) {
sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
if (!sbusFrameData->done) {
return RX_FRAME_PENDING;
}
sbusFrameDone = false;
sbusFrameData->done = false;
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags = 0;
debug[1] = sbusFrame.frame.flags;
#endif
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
sbusChannelData[0] = sbusFrame.frame.chan0;
sbusChannelData[1] = sbusFrame.frame.chan1;
sbusChannelData[2] = sbusFrame.frame.chan2;
sbusChannelData[3] = sbusFrame.frame.chan3;
sbusChannelData[4] = sbusFrame.frame.chan4;
sbusChannelData[5] = sbusFrame.frame.chan5;
sbusChannelData[6] = sbusFrame.frame.chan6;
sbusChannelData[7] = sbusFrame.frame.chan7;
sbusChannelData[8] = sbusFrame.frame.chan8;
sbusChannelData[9] = sbusFrame.frame.chan9;
sbusChannelData[10] = sbusFrame.frame.chan10;
sbusChannelData[11] = sbusFrame.frame.chan11;
sbusChannelData[12] = sbusFrame.frame.chan12;
sbusChannelData[13] = sbusFrame.frame.chan13;
sbusChannelData[14] = sbusFrame.frame.chan14;
sbusChannelData[15] = sbusFrame.frame.chan15;
if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_17) {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
sbusFrameData->stateFlags |= SBUS_STATE_SIGNALLOSS;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
}
if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
sbusFrameData->stateFlags |= SBUS_STATE_FAILSAFE;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
}
if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
}
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
debug[0] = sbusStateFlags;
#endif
}
if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
// internal failsafe enabled and rx failsafe flag set
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags |= SBUS_STATE_FAILSAFE;
debug[0] = sbusStateFlags;
#endif
// RX *should* still be sending valid channel data, so use it.
return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
}
#ifdef DEBUG_SBUS_PACKETS
debug[0] = sbusStateFlags;
#endif
return RX_FRAME_COMPLETE;
}
static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{
UNUSED(rxRuntimeConfig);
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
return (0.625f * sbusChannelData[chan]) + 880;
return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
}
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
}
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
static sbusFrameData_t sbusFrameData;
rxRuntimeConfig->channelData = sbusChannelData;
rxRuntimeConfig->frameData = &sbusFrameData;
sbusChannelsInit(rxConfig, rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFn = sbusReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
@ -251,10 +186,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL,
sbusDataReceive,
NULL,
&sbusFrameData,
SBUS_BAUDRATE,
portShared ? MODE_RXTX : MODE_RX,
SBUS_PORT_OPTIONS | (rxConfig->sbus_inversion ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
);
#ifdef USE_TELEMETRY
@ -265,4 +200,4 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
return sBusPort != NULL;
}
#endif // USE_SERIALRX_SBUS
#endif

1
src/main/rx/sbus.h
View file

@ -17,5 +17,4 @@
#pragma once
uint8_t sbusFrameStatus(void);
bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

96
src/main/rx/sbus_channels.c Normal file
View file

@ -0,0 +1,96 @@
/*
* 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 "platform.h"
#ifdef USE_SERIAL_RX
#include "common/utils.h"
#include "rx/rx.h"
#include "rx/sbus_channels.h"
#define DEBUG_SBUS_FRAME_INTERVAL 3
#define SBUS_FLAG_CHANNEL_17 (1 << 0)
#define SBUS_FLAG_CHANNEL_18 (1 << 1)
#define SBUS_DIGITAL_CHANNEL_MIN 173
#define SBUS_DIGITAL_CHANNEL_MAX 1812
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
{
uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
sbusChannelData[0] = channels->chan0;
sbusChannelData[1] = channels->chan1;
sbusChannelData[2] = channels->chan2;
sbusChannelData[3] = channels->chan3;
sbusChannelData[4] = channels->chan4;
sbusChannelData[5] = channels->chan5;
sbusChannelData[6] = channels->chan6;
sbusChannelData[7] = channels->chan7;
sbusChannelData[8] = channels->chan8;
sbusChannelData[9] = channels->chan9;
sbusChannelData[10] = channels->chan10;
sbusChannelData[11] = channels->chan11;
sbusChannelData[12] = channels->chan12;
sbusChannelData[13] = channels->chan13;
sbusChannelData[14] = channels->chan14;
sbusChannelData[15] = channels->chan15;
if (channels->flags & SBUS_FLAG_CHANNEL_17) {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
}
if (channels->flags & SBUS_FLAG_CHANNEL_18) {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
}
if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
}
if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
// internal failsafe enabled and rx failsafe flag set
// RX *should* still be sending valid channel data, so use it.
return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
}
return RX_FRAME_COMPLETE;
}
static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
}
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
rxRuntimeConfig->channelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
}
}
#endif

52
src/main/rx/sbus_channels.h Normal file
View file

@ -0,0 +1,52 @@
/*
* 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 <stdint.h>
#define SBUS_MAX_CHANNEL 18
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
typedef struct sbusChannels_s {
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
unsigned int chan12 : 11;
unsigned int chan13 : 11;
unsigned int chan14 : 11;
unsigned int chan15 : 11;
uint8_t flags;
} __attribute__((__packed__)) sbusChannels_t;
#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

4
src/main/rx/spektrum.c
View file

@ -112,8 +112,10 @@ static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
uint8_t spektrumFrameStatus(void)
static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) {
return RX_FRAME_PENDING;
}

1
src/main/rx/spektrum.h
View file

@ -20,6 +20,5 @@
#define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10
uint8_t spektrumFrameStatus(void);
void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

4
src/main/rx/sumd.c
View file

@ -114,8 +114,10 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData)
#define SUMD_FRAME_STATE_OK 0x01
#define SUMD_FRAME_STATE_FAILSAFE 0x81
uint8_t sumdFrameStatus(void)
static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
uint8_t channelIndex;
uint8_t frameStatus = RX_FRAME_PENDING;

1
src/main/rx/sumd.h
View file

@ -17,5 +17,4 @@
#pragma once
uint8_t sumdFrameStatus(void);
bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

4
src/main/rx/sumh.c
View file

@ -82,8 +82,10 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData)
}
}
uint8_t sumhFrameStatus(void)
uint8_t sumhFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
uint8_t channelIndex;
if (!sumhFrameDone) {

1
src/main/rx/sumh.h
View file

@ -17,5 +17,4 @@
#pragma once
uint8_t sumhFrameStatus(void);
bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

4
src/main/rx/uib_rx.c
View file

@ -81,8 +81,10 @@ static uint16_t rxUIBReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint
}
}
static uint8_t rxUIBFrameStatus(void)
static uint8_t rxUIBFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!uavInterconnectBusIsInitialized()) {
return RX_FRAME_FAILSAFE;
}

4
src/main/rx/xbus.c
View file

@ -255,8 +255,10 @@ static void xBusDataReceive(uint16_t c, void *rxCallbackData)
}
// Indicate time to read a frame from the data...
uint8_t xBusFrameStatus(void)
static uint8_t xBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!xBusFrameReceived) {
return RX_FRAME_PENDING;
}

1
src/main/rx/xbus.h
View file

@ -18,4 +18,3 @@
#pragma once
bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
uint8_t xBusFrameStatus(void);

10
src/main/sensors/battery.c
View file

@ -234,6 +234,11 @@ bool batteryUsesCapacityThresholds(void)
return batteryUseCapacityThresholds;
}
bool isBatteryVoltageConfigured(void)
{
return feature(FEATURE_VBAT);
}
uint16_t getBatteryVoltage(void)
{
return vbat;
@ -267,6 +272,11 @@ uint32_t getBatteryRemainingCapacity(void)
return batteryRemainingCapacity;
}
bool isAmperageConfigured(void)
{
return batteryConfig()->current.type != CURRENT_SENSOR_NONE;
}
int32_t getAmperage(void)
{
return amperage;

2
src/main/sensors/battery.h
View file

@ -81,6 +81,7 @@ bool batteryUsesCapacityThresholds(void);
void batteryUpdate(uint32_t vbatTimeDelta);
void batteryInit(void);
bool isBatteryVoltageConfigured(void);
uint16_t getBatteryVoltage(void);
uint16_t getBatteryVoltageLatestADC(void);
uint16_t getBatteryWarningVoltage(void);
@ -88,6 +89,7 @@ uint8_t getBatteryCellCount(void);
uint16_t getBatteryAverageCellVoltage(void);
uint32_t getBatteryRemainingCapacity(void);
bool isAmperageConfigured(void);
int32_t getAmperage(void);
int32_t getAmperageLatestADC(void);
int32_t getPower(void);

1
src/main/target/ALIENFLIGHTF4/config.c
View file

@ -70,7 +70,6 @@ void targetConfiguration(void)
rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->sbus_inversion = 0;
serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY;
telemetryConfigMutable()->telemetry_inversion = 0;
featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);

1
src/main/target/ALIENFLIGHTNGF7/config.c
View file

@ -72,7 +72,6 @@ void targetConfiguration(void)
rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->sbus_inversion = 0;
serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY;
telemetryConfigMutable()->telemetry_inversion = 0;
featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);

3
src/main/target/SPRACINGF3NEO/config.c
View file

@ -24,8 +24,6 @@
#include "io/serial.h"
#include "rx/rx.h"
#include "sensors/sensors.h"
#include "sensors/compass.h"
#include "sensors/barometer.h"
@ -36,7 +34,6 @@ void targetConfiguration(void)
{
barometerConfigMutable()->baro_hardware = BARO_AUTODETECT;
compassConfigMutable()->mag_hardware = MAG_AUTODETECT;
rxConfigMutable()->sbus_inversion = 1;
serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything.
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS;

2
src/main/target/SPRACINGF4EVO/config.c
View file

@ -23,7 +23,6 @@
#ifdef TARGET_CONFIG
#include "io/serial.h"
#include "rx/rx.h"
#include "sensors/barometer.h"
#include "telemetry/telemetry.h"
@ -31,7 +30,6 @@
void targetConfiguration(void)
{
barometerConfigMutable()->baro_hardware = BARO_BMP280;
rxConfigMutable()->sbus_inversion = 1;
serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything.
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
telemetryConfigMutable()->telemetry_inversion = true;

2
src/main/target/common.h
View file

@ -30,6 +30,7 @@
#define USE_SERIALRX_SPEKTRUM // Cheap and fairly common protocol
#define USE_SERIALRX_SBUS // Very common protocol
#define USE_SERIALRX_IBUS // Cheap FlySky & Turnigy receivers
#define USE_SERIALRX_FPORT
#define COMMON_DEFAULT_FEATURES (FEATURE_TX_PROF_SEL)
@ -91,6 +92,7 @@
#define USE_TELEMETRY_MAVLINK
#define USE_TELEMETRY_SMARTPORT
#define USE_TELEMETRY_CRSF
#define USE_MSP_OVER_TELEMETRY
// These are rather exotic serial protocols
#define USE_RX_MSP
#define USE_SERIALRX_SUMD

2
src/main/telemetry/frsky.c
View file

@ -395,7 +395,7 @@ static void sendVoltageAmp(void)
} else {
uint16_t voltage = (vbat * 11) / 21;
uint16_t vfasVoltage;
if (telemetryConfig()->frsky_vfas_cell_voltage) {
if (telemetryConfig()->report_cell_voltage) {
vfasVoltage = voltage / getBatteryCellCount();
} else {
vfasVoltage = voltage;

3
src/main/telemetry/ltm.c
View file

@ -81,7 +81,6 @@
#define LTM_CYCLETIME 100
#define LTM_SCHEDULE_SIZE (1000/LTM_CYCLETIME)
extern uint16_t rssi; // FIXME dependency on mw.c
static serialPort_t *ltmPort;
static serialPortConfig_t *portConfig;
static bool ltmEnabled;
@ -187,7 +186,7 @@ void ltm_sframe(sbuf_t *dst)
sbufWriteU8(dst, 'S');
sbufWriteU16(dst, getBatteryVoltage() * 10); //vbat converted to mv
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // current mAh (65535 mAh max)
sbufWriteU8(dst, (uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar)
sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023)); // scaled RSSI (uchar)
#if defined(USE_PITOT)
sbufWriteU8(dst, sensors(SENSOR_PITOT) ? pitot.airSpeed / 100.0f : 0); // in m/s
#else

4
src/main/telemetry/mavlink.c
View file

@ -83,8 +83,6 @@
#define TELEMETRY_MAVLINK_MAXRATE 50
#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
extern uint16_t rssi; // FIXME dependency on mw.c
static serialPort_t *mavlinkPort = NULL;
static serialPortConfig_t *portConfig;
@ -272,7 +270,7 @@ void mavlinkSendRCChannelsAndRSSI(void)
// chan8_raw RC channel 8 value, in microseconds
(rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
scaleRange(rssi, 0, 1023, 0, 255));
scaleRange(getRSSI(), 0, 1023, 0, 255));
mavlinkSendMessage();
}

225
src/main/telemetry/msp_shared.c Normal file
View file

@ -0,0 +1,225 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_MSP_OVER_TELEMETRY)
#include "build/build_config.h"
#include "common/utils.h"
#include "fc/fc_msp.h"
#include "msp/msp.h"
#include "telemetry/crsf.h"
#include "telemetry/msp_shared.h"
#include "telemetry/smartport.h"
#define TELEMETRY_MSP_VERSION 1
#define TELEMETRY_MSP_VER_SHIFT 5
#define TELEMETRY_MSP_VER_MASK (0x7 << TELEMETRY_MSP_VER_SHIFT)
#define TELEMETRY_MSP_ERROR_FLAG (1 << 5)
#define TELEMETRY_MSP_START_FLAG (1 << 4)
#define TELEMETRY_MSP_SEQ_MASK 0x0F
#define TELEMETRY_MSP_RES_ERROR (-10)
enum {
TELEMETRY_MSP_VER_MISMATCH=0,
TELEMETRY_MSP_CRC_ERROR=1,
TELEMETRY_MSP_ERROR=2
};
STATIC_UNIT_TESTED uint8_t checksum = 0;
STATIC_UNIT_TESTED mspPackage_t mspPackage;
static mspRxBuffer_t mspRxBuffer;
static mspTxBuffer_t mspTxBuffer;
static mspPacket_t mspRxPacket;
static mspPacket_t mspTxPacket;
void initSharedMsp(void)
{
mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer;
mspPackage.requestPacket = &mspRxPacket;
mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
mspPackage.responsePacket = &mspTxPacket;
mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
}
static void processMspPacket(void)
{
mspPackage.responsePacket->cmd = 0;
mspPackage.responsePacket->result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
mspPostProcessFnPtr mspPostProcessFn = NULL;
if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
}
if (mspPostProcessFn) {
mspPostProcessFn(NULL);
}
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
}
void sendMspErrorResponse(uint8_t error, int16_t cmd)
{
mspPackage.responsePacket->cmd = cmd;
mspPackage.responsePacket->result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
sbufWriteU8(&mspPackage.responsePacket->buf, error);
mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
}
bool handleMspFrame(uint8_t *frameStart, int frameLength)
{
static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0;
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
mspStarted = 0;
}
if (mspStarted == 0) {
initSharedMsp();
}
mspPacket_t *packet = mspPackage.requestPacket;
sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
const uint8_t header = sbufReadU8(frameBuf);
const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
if (version != TELEMETRY_MSP_VERSION) {
sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
return true;
}
if (header & TELEMETRY_MSP_START_FLAG) {
// first packet in sequence
uint8_t mspPayloadSize = sbufReadU8(frameBuf);
packet->cmd = sbufReadU8(frameBuf);
packet->result = 0;
packet->buf.ptr = mspPackage.requestBuffer;
packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
checksum = mspPayloadSize ^ packet->cmd;
mspStarted = 1;
} else if (!mspStarted) {
// no start packet yet, throw this one away
return false;
} else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
// packet loss detected!
mspStarted = 0;
return false;
}
const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
uint8_t payload[frameBytesRemaining];
if (bufferBytesRemaining >= frameBytesRemaining) {
sbufReadData(frameBuf, payload, frameBytesRemaining);
sbufAdvance(frameBuf, frameBytesRemaining);
sbufWriteData(rxBuf, payload, frameBytesRemaining);
lastSeq = seqNumber;
return false;
} else {
sbufReadData(frameBuf, payload, bufferBytesRemaining);
sbufAdvance(frameBuf, bufferBytesRemaining);
sbufWriteData(rxBuf, payload, bufferBytesRemaining);
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
while (sbufBytesRemaining(rxBuf)) {
checksum ^= sbufReadU8(rxBuf);
}
if (checksum != *frameBuf->ptr) {
mspStarted = 0;
sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
return true;
}
}
mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
processMspPacket();
return true;
}
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)
{
static uint8_t checksum = 0;
static uint8_t seq = 0;
uint8_t payloadOut[payloadSize];
sbuf_t payload;
sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize);
sbuf_t *txBuf = &mspPackage.responsePacket->buf;
// detect first reply packet
if (txBuf->ptr == mspPackage.responseBuffer) {
// header
uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
if (mspPackage.responsePacket->result < 0) {
head |= TELEMETRY_MSP_ERROR_FLAG;
}
sbufWriteU8(payloadBuf, head);
uint8_t size = sbufBytesRemaining(txBuf);
sbufWriteU8(payloadBuf, size);
} else {
// header
sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
}
const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf);
uint8_t frame[payloadBytesRemaining];
if (bufferBytesRemaining >= payloadBytesRemaining) {
sbufReadData(txBuf, frame, payloadBytesRemaining);
sbufAdvance(txBuf, payloadBytesRemaining);
sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
responseFn(payloadOut);
return true;
} else {
sbufReadData(txBuf, frame, bufferBytesRemaining);
sbufAdvance(txBuf, bufferBytesRemaining);
sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
while (sbufBytesRemaining(txBuf)) {
checksum ^= sbufReadU8(txBuf);
}
sbufWriteU8(payloadBuf, checksum);
while (sbufBytesRemaining(payloadBuf)>1) {
sbufWriteU8(payloadBuf, 0);
}
}
responseFn(payloadOut);
return false;
}
#endif

34
src/main/telemetry/msp_shared.h Normal file
View file

@ -0,0 +1,34 @@
#pragma once
#include "common/streambuf.h"
#include "telemetry/crsf.h"
#include "telemetry/smartport.h"
typedef void (*mspResponseFnPtr)(uint8_t *payload);
struct mspPacket_s;
typedef struct mspPackage_s {
sbuf_t requestFrame;
uint8_t *requestBuffer;
uint8_t *responseBuffer;
struct mspPacket_s *requestPacket;
struct mspPacket_s *responsePacket;
} mspPackage_t;
typedef union mspRxBuffer_u {
uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
#if 0
uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
#endif
} mspRxBuffer_t;
typedef union mspTxBuffer_u {
uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
#if 0
uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
#endif
} mspTxBuffer_t;
void initSharedMsp(void);
bool handleMspFrame(uint8_t *frameStart, int frameLength);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

720
src/main/telemetry/smartport.c
View file

@ -5,6 +5,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -17,65 +18,49 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/compass/compass.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "drivers/serial.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_msp.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/serial.h"
#include "navigation/navigation.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/pitotmeter.h"
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "telemetry/smartport.h"
#include "telemetry/msp_shared.h"
#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
enum
{
SPSTATE_UNINITIALIZED,
SPSTATE_INITIALIZED,
SPSTATE_WORKING,
};
enum
{
FSSP_START_STOP = 0x7E,
FSSP_DLE = 0x7D,
FSSP_DLE_XOR = 0x20,
FSSP_DATA_FRAME = 0x10,
FSSP_MSPC_FRAME = 0x30, // MSP client frame
FSSP_MSPS_FRAME = 0x32, // MSP server frame
// ID of sensor. Must be something that is polled by FrSky RX
FSSP_SENSOR_ID1 = 0x1B,
FSSP_SENSOR_ID2 = 0x0D,
FSSP_SENSOR_ID3 = 0x34,
FSSP_SENSOR_ID4 = 0x67,
// there are 32 ID's polled by smartport master
// remaining 3 bits are crc (according to comments in openTx code)
};
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
enum
{
FSSP_DATAID_SPEED = 0x0830 ,
@ -87,6 +72,7 @@ enum
FSSP_DATAID_ADC1 = 0xF102 ,
FSSP_DATAID_ADC2 = 0xF103 ,
FSSP_DATAID_LATLONG = 0x0800 ,
FSSP_DATAID_CAP_USED = 0x0600 ,
FSSP_DATAID_VARIO = 0x0110 ,
FSSP_DATAID_CELLS = 0x0300 ,
FSSP_DATAID_CELLS_LAST = 0x030F ,
@ -100,10 +86,10 @@ enum
FSSP_DATAID_GPS_ALT = 0x0820 ,
FSSP_DATAID_ASPD = 0x0A00 ,
FSSP_DATAID_A3 = 0x0900 ,
FSSP_DATAID_A4 = 0x0910 ,
FSSP_DATAID_A4 = 0x0910
};
const uint16_t smartPortDataIdTable[] = {
const uint16_t frSkyDataIdTable[] = {
FSSP_DATAID_SPEED ,
FSSP_DATAID_VFAS ,
FSSP_DATAID_CURRENT ,
@ -114,6 +100,7 @@ const uint16_t smartPortDataIdTable[] = {
//FSSP_DATAID_ADC2 ,
FSSP_DATAID_LATLONG ,
FSSP_DATAID_LATLONG , // twice
//FSSP_DATAID_CAP_USED ,
FSSP_DATAID_VARIO ,
//FSSP_DATAID_CELLS ,
//FSSP_DATAID_CELLS_LAST,
@ -126,526 +113,317 @@ const uint16_t smartPortDataIdTable[] = {
FSSP_DATAID_HOME_DIST ,
FSSP_DATAID_GPS_ALT ,
FSSP_DATAID_ASPD ,
//FSSP_DATAID_A3 ,
FSSP_DATAID_A3 ,
FSSP_DATAID_A4 ,
0
};
#define __USE_C99_MATH // for roundf()
#define SMARTPORT_BAUD 57600
#define SMARTPORT_UART_MODE MODE_RXTX
#define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
#define SMARTPORT_DATA_LENGTH (sizeof(smartPortDataIdTable) / sizeof(smartPortDataIdTable[0]))
static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
static serialPortConfig_t *portConfig;
static bool smartPortTelemetryEnabled = false;
static portSharing_e smartPortPortSharing;
char smartPortState = SPSTATE_UNINITIALIZED;
static bool smartPortHasRequest = false;
enum
{
TELEMETRY_STATE_UNINITIALIZED,
TELEMETRY_STATE_INITIALIZED_SERIAL,
TELEMETRY_STATE_INITIALIZED_EXTERNAL,
};
static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
static uint8_t smartPortIdCnt = 0;
static uint32_t smartPortLastRequestTime = 0;
typedef struct smartPortFrame_s {
uint8_t sensorId;
uint8_t frameId;
uint16_t valueId;
uint32_t data;
smartPortPayload_t payload;
uint8_t crc;
} __attribute__((packed)) smartPortFrame_t;
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
#define SMARTPORT_TX_BUF_SIZE 256
#define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
static smartPortWriteFrameFn *smartPortWriteFrame;
static smartPortFrame_t smartPortRxBuffer;
static uint8_t smartPortRxBytes = 0;
static bool smartPortFrameReceived = false;
#define SMARTPORT_MSP_VERSION 1
#define SMARTPORT_MSP_VER_SHIFT 5
#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT)
#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT)
#define SMARTPORT_MSP_ERROR_FLAG (1 << 5)
#define SMARTPORT_MSP_START_FLAG (1 << 4)
#define SMARTPORT_MSP_SEQ_MASK 0x0F
#define SMARTPORT_MSP_RX_BUF_SIZE 64
static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE];
static mspPacket_t smartPortMspReply;
#if defined(USE_MSP_OVER_TELEMETRY)
static bool smartPortMspReplyPending = false;
#endif
#define SMARTPORT_MSP_RES_ERROR (-10)
enum {
SMARTPORT_MSP_VER_MISMATCH = 0,
SMARTPORT_MSP_CRC_ERROR = 1,
SMARTPORT_MSP_ERROR = 2,
};
static void smartPortDataReceive(uint16_t c)
smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
{
static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
static uint8_t smartPortRxBytes = 0;
static bool skipUntilStart = true;
static bool awaitingSensorId = false;
static bool byteStuffing = false;
static uint16_t checksum = 0;
uint32_t now = millis();
if (c == FSSP_START_STOP) {
*clearToSend = false;
smartPortRxBytes = 0;
smartPortHasRequest = false;
awaitingSensorId = true;
skipUntilStart = false;
return;
} else if (skipUntilStart)
return;
uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer;
if (smartPortRxBytes == 0) {
if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
return NULL;
} else if (skipUntilStart) {
return NULL;
}
// our slot is starting...
smartPortLastRequestTime = now;
smartPortHasRequest = true;
if (awaitingSensorId) {
awaitingSensorId = false;
if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
// our slot is starting, no need to decode more
*clearToSend = true;
skipUntilStart = true;
} else if (c == FSSP_SENSOR_ID2) {
rxBuffer[smartPortRxBytes++] = c;
checksum = 0;
} else
} else {
skipUntilStart = true;
}
else {
} else {
if (c == FSSP_DLE) {
byteStuffing = true;
return;
}
if (byteStuffing) {
return NULL;
} else if (byteStuffing) {
c ^= FSSP_DLE_XOR;
byteStuffing = false;
}
rxBuffer[smartPortRxBytes++] = c;
if (smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
if (c == (0xFF - checksum))
smartPortFrameReceived = true;
skipUntilStart = true;
} else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) {
if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
rxBuffer[smartPortRxBytes++] = (uint8_t)c;
checksum += c;
checksum += checksum >> 8;
checksum &= 0x00FF;
if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
skipUntilStart = true;
return (smartPortPayload_t *)&rxBuffer;
}
} else {
skipUntilStart = true;
checksum += c;
checksum = (checksum & 0xFF) + (checksum >> 8);
if (checksum == 0xFF) {
return (smartPortPayload_t *)&rxBuffer;
}
}
}
static void smartPortSendByte(uint8_t c, uint16_t *crcp)
return NULL;
}
void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
{
// smart port escape sequence
if (c == FSSP_DLE || c == FSSP_START_STOP) {
serialWrite(smartPortSerialPort, FSSP_DLE);
serialWrite(smartPortSerialPort, c ^ FSSP_DLE_XOR);
} else
serialWrite(smartPortSerialPort, c);
if (crcp == NULL)
return;
uint16_t crc = *crcp;
crc += c;
crc += crc >> 8;
crc &= 0x00FF;
*crcp = crc;
serialWrite(port, FSSP_DLE);
serialWrite(port, c ^ FSSP_DLE_XOR);
} else {
serialWrite(port, c);
}
static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data)
if (checksum != NULL) {
*checksum += c;
}
}
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
{
uint16_t crc = 0;
smartPortSendByte(frameId, &crc);
for (unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++)
smartPortSendByte(*data++, &crc);
smartPortSendByte(0xFF - (uint8_t)crc, NULL);
uint8_t *data = (uint8_t *)payload;
for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
smartPortSendByte(*data++, &checksum, port);
}
checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
smartPortSendByte((uint8_t)checksum, NULL, port);
}
static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
{
smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
}
static void smartPortSendPackage(uint16_t id, uint32_t val)
{
uint8_t payload[SMARTPORT_PAYLOAD_SIZE];
uint8_t *dst = payload;
*dst++ = id & 0xFF;
*dst++ = id >> 8;
*dst++ = val & 0xFF;
*dst++ = (val >> 8) & 0xFF;
*dst++ = (val >> 16) & 0xFF;
*dst++ = (val >> 24) & 0xFF;
smartPortPayload_t payload;
payload.frameId = FSSP_DATA_FRAME;
payload.valueId = id;
payload.data = val;
smartPortSendPackageEx(FSSP_DATA_FRAME,payload);
smartPortHasRequest = false;
smartPortWriteFrame(&payload);
}
void initSmartPortTelemetry(void)
bool initSmartPortTelemetry(void)
{
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
if (portConfig) {
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
smartPortWriteFrame = smartPortWriteFrameInternal;
telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
}
void freeSmartPortTelemetryPort(void)
return true;
}
return false;
}
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
{
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
smartPortWriteFrame = smartPortWriteFrameExternal;
telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
return true;
}
return false;
}
static void freeSmartPortTelemetryPort(void)
{
closeSerialPort(smartPortSerialPort);
smartPortSerialPort = NULL;
smartPortState = SPSTATE_UNINITIALIZED;
smartPortTelemetryEnabled = false;
}
void configureSmartPortTelemetryPort(void)
static void configureSmartPortTelemetryPort(void)
{
portOptions_t portOptions;
if (!portConfig)
return;
if (telemetryConfig()->smartportUartUnidirectional)
portOptions = SERIAL_UNIDIR;
else
portOptions = SERIAL_BIDIR;
if (telemetryConfig()->telemetry_inversion)
portOptions |= SERIAL_INVERTED;
if (portConfig) {
portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inversion ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
if (!smartPortSerialPort)
return;
smartPortState = SPSTATE_INITIALIZED;
smartPortTelemetryEnabled = true;
smartPortLastRequestTime = millis();
}
bool canSendSmartPortTelemetry(void)
{
return smartPortSerialPort && (smartPortState == SPSTATE_INITIALIZED || smartPortState == SPSTATE_WORKING);
}
void checkSmartPortTelemetryState(void)
{
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(smartPortPortSharing);
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
if (newTelemetryEnabledValue == smartPortTelemetryEnabled)
return;
if (newTelemetryEnabledValue)
if (enableSerialTelemetry && !smartPortSerialPort) {
configureSmartPortTelemetryPort();
else
} else if (!enableSerialTelemetry && smartPortSerialPort) {
freeSmartPortTelemetryPort();
}
}
}
static void initSmartPortMspReply(int16_t cmd)
#if defined(USE_MSP_OVER_TELEMETRY)
static void smartPortSendMspResponse(uint8_t *data) {
smartPortPayload_t payload;
payload.frameId = FSSP_MSPS_FRAME;
memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
smartPortWriteFrame(&payload);
}
#endif
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
{
smartPortMspReply.buf.ptr = smartPortMspTxBuffer;
smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer);
smartPortMspReply.cmd = cmd;
smartPortMspReply.result = 0;
}
static void processMspPacket(mspPacket_t* packet)
{
initSmartPortMspReply(0);
if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR)
sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR);
// change streambuf direction
sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
smartPortMspReplyPending = true;
}
/**
* Request frame format:
* - Header: 1 byte
* - Reserved: 2 bits (future use)
* - Error-flag: 1 bit
* - Start-flag: 1 bit
* - CSeq: 4 bits
*
* - MSP payload:
* - if Error-flag == 0:
* - size: 1 byte
* - payload
* - CRC (request type included)
* - if Error-flag == 1:
* - size: 1 byte (== 1)
* - error: 1 Byte
* - 0: Version mismatch (type=0)
* - 1: Sequence number error
* - 2: MSP error
* - CRC (request type included)
*/
bool smartPortSendMspReply(void)
{
static uint8_t checksum = 0;
static uint8_t seq = 0;
uint8_t packet[SMARTPORT_PAYLOAD_SIZE];
uint8_t* p = packet;
uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
sbuf_t* txBuf = &smartPortMspReply.buf;
// detect first reply packet
if (txBuf->ptr == smartPortMspTxBuffer) {
// header
uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK);
if (smartPortMspReply.result < 0)
head |= SMARTPORT_MSP_ERROR_FLAG;
*p++ = head;
uint8_t size = sbufBytesRemaining(txBuf);
*p++ = size;
checksum = size ^ smartPortMspReply.cmd;
} else {
// header
*p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK);
}
while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) {
*p = sbufReadU8(txBuf);
checksum ^= *p++; // MSP checksum
}
// to be continued...
if (p == end) {
smartPortSendPackageEx(FSSP_MSPS_FRAME, packet);
return true;
}
// nothing left in txBuf,
// append the MSP checksum
*p++ = checksum;
// pad with zeros
while (p < end)
*p++ = 0;
smartPortSendPackageEx(FSSP_MSPS_FRAME, packet);
return false;
}
void smartPortSendErrorReply(uint8_t error, int16_t cmd)
{
initSmartPortMspReply(cmd);
sbufWriteU8(&smartPortMspReply.buf, error);
smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR;
sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
smartPortMspReplyPending = true;
}
/**
* Request frame format:
* - Header: 1 byte
* - Version: 3 bits
* - Start-flag: 1 bit
* - CSeq: 4 bits
*
* - MSP payload:
* - Size: 1 Byte
* - Type: 1 Byte
* - payload...
* - CRC
*/
void handleSmartPortMspFrame(smartPortFrame_t* sp_frame)
{
static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0;
static uint8_t checksum = 0;
static mspPacket_t cmd;
// re-assemble MSP frame & forward to MSP port when complete
uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET;
uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
uint8_t head = *p++;
uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK;
uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT;
if (version != SMARTPORT_MSP_VERSION) {
mspStarted = 0;
smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0);
return;
}
// check start-flag
if (head & SMARTPORT_MSP_START_FLAG) {
//TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error!
uint8_t p_size = *p++;
cmd.cmd = *p++;
cmd.result = 0;
cmd.buf.ptr = mspBuffer;
cmd.buf.end = mspBuffer + p_size;
checksum = p_size ^ cmd.cmd;
mspStarted = 1;
} else if (!mspStarted)
return; // no start packet yet, throw this one away
else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) {
// packet loss detected!
mspStarted = 0;
return;
}
// copy payload bytes
while ((p < end) && sbufBytesRemaining(&cmd.buf)) {
checksum ^= *p;
sbufWriteU8(&cmd.buf,*p++);
}
// reached end of smart port frame
if (p == end) {
lastSeq = seq;
return;
}
// last byte must be the checksum
if (checksum != *p) {
mspStarted = 0;
smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd);
return;
}
// end of MSP packet reached
mspStarted = 0;
sbufSwitchToReader(&cmd.buf,mspBuffer);
processMspPacket(&cmd);
}
void handleSmartPortTelemetry(void)
{
uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled)
return;
if (!canSendSmartPortTelemetry())
return;
while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
uint8_t c = serialRead(smartPortSerialPort);
smartPortDataReceive(c);
}
if (smartPortFrameReceived) {
smartPortFrameReceived = false;
if (payload) {
// do not check the physical ID here again
// unless we start receiving other sensors' packets
if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME)
handleSmartPortMspFrame(&smartPortRxBuffer); // Pass only the payload: skip sensorId & frameId
#if defined(USE_MSP_OVER_TELEMETRY)
if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) {
// Pass only the payload: skip frameId
uint8_t *frameStart = (uint8_t *)&payload->valueId;
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
}
#endif
}
while (smartPortHasRequest) {
bool doRun = true;
while (doRun && *clearToSend) {
// Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
smartPortHasRequest = false;
if (requestTimeout) {
if (millis() >= *requestTimeout) {
*clearToSend = false;
return;
}
} else {
doRun = false;
}
#if defined(USE_MSP_OVER_TELEMETRY)
if (smartPortMspReplyPending) {
smartPortMspReplyPending = smartPortSendMspReply();
smartPortHasRequest = 0;
smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
*clearToSend = false;
return;
}
#endif
// we can send back any data we want, our table keeps track of the order and frequency of each data type we send
uint16_t id = frSkyDataIdTable[smartPortIdCnt];
if (id == 0) { // end of table reached, loop back
smartPortIdCnt = 0;
id = frSkyDataIdTable[smartPortIdCnt];
}
smartPortIdCnt++;
if (smartPortIdCnt >= SMARTPORT_DATA_LENGTH)
smartPortIdCnt = 0; // end of table reached, loop back
uint16_t id = smartPortDataIdTable[smartPortIdCnt];
switch (id) {
#ifdef USE_GPS
case FSSP_DATAID_SPEED :
//convert to knots: 1cm/s = 0.0194384449 knots
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
smartPortSendPackage(id, (uint32_t)gpsSol.groundSpeed * 1944 / 100);
break;
#endif
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT)) {
uint16_t vfasVoltage;
uint16_t vbat = getBatteryVoltage();
if (telemetryConfig()->frsky_vfas_cell_voltage)
vfasVoltage = vbat / getBatteryCellCount();
else
vfasVoltage = vbat;
if (isBatteryVoltageConfigured()) {
uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
smartPortSendPackage(id, vfasVoltage);
*clearToSend = false;
}
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_CURRENT_METER))
if (isAmperageConfigured()) {
smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
*clearToSend = false;
}
break;
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO))
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter
*clearToSend = false;
}
break;
case FSSP_DATAID_FUEL :
if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT)
if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON
else if (feature(FEATURE_CURRENT_METER))
*clearToSend = false;
} else if (isAmperageConfigured()) {
smartPortSendPackage(id, (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn()));
*clearToSend = false;
}
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);
}
break;
#endif
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO))
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s
*clearToSend = false;
}
break;
case FSSP_DATAID_HEADING :
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
*clearToSend = false;
break;
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
*clearToSend = false;
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
*clearToSend = false;
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
*clearToSend = false;
break;
case FSSP_DATAID_T1 :
{
@ -692,7 +470,8 @@ void handleSmartPortTelemetry(void)
if (FLIGHT_MODE(FAILSAFE_MODE))
tmpi += 40000;
smartPortSendPackage(id, tmpi);
smartPortSendPackage(id, (uint32_t)tmpi);
*clearToSend = false;
break;
}
case FSSP_DATAID_T2 :
@ -715,36 +494,103 @@ void handleSmartPortTelemetry(void)
tmpi += 4000;
smartPortSendPackage(id, tmpi);
*clearToSend = false;
#endif
} else if (feature(FEATURE_GPS))
} else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
*clearToSend = false;
}
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_HOME_DIST :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_distanceToHome);
*clearToSend = false;
}
break;
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, gpsSol.llh.alt); // cm
*clearToSend = false;
}
break;
#endif
#ifdef USE_PITOT
case FSSP_DATAID_ASPD :
if (sensors(SENSOR_PITOT))
smartPortSendPackage(id, pitot.airSpeed * 0.194384449f); // cm/s to knots*10
break;
#endif
//case FSSP_DATAID_A3 :
case FSSP_DATAID_A4 :
if (feature(FEATURE_VBAT))
if (isBatteryVoltageConfigured()) {
smartPortSendPackage(id, getBatteryAverageCellVoltage());
*clearToSend = false;
}
break;
case FSSP_DATAID_ASPD :
#ifdef USE_PITOT
if (sensors(SENSOR_PITOT)) {
smartPortSendPackage(id, pitot.airSpeed * 0.194384449f); // cm/s to knots*1
*clearToSend = false;
}
#endif
break;
default:
break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
// if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
}
}
}
static bool serialCheckQueueEmpty(void)
{
return (serialRxBytesWaiting(smartPortSerialPort) == 0);
}
void handleSmartPortTelemetry(void)
{
static bool clearToSend = false;
static volatile timeUs_t lastTelemetryFrameReceivedUs;
static smartPortPayload_t *payload = NULL;
const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
uint8_t c = serialRead(smartPortSerialPort);
payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
if (payload) {
lastTelemetryFrameReceivedUs = micros();
}
}
if (cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
payload = NULL;
}
}
}
#endif

49
src/main/telemetry/smartport.h
View file

@ -7,10 +7,51 @@
#pragma once
void initSmartPortTelemetry(void);
#include <stdbool.h>
#include <stdint.h>
#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;
uint32_t data;
} __attribute__((packed)) smartPortPayload_t;
typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
typedef bool smartPortCheckQueueEmptyFn(void);
bool initSmartPortTelemetry(void);
void checkSmartPortTelemetryState(void);
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal);
void handleSmartPortTelemetry(void);
void checkSmartPortTelemetryState(void);
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout);
void configureSmartPortTelemetryPort(void);
void freeSmartPortTelemetryPort(void);
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);

5
src/main/telemetry/telemetry.c
View file

@ -65,12 +65,13 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.frsky_coordinate_format = FRSKY_FORMAT_DMS,
.frsky_unit = FRSKY_UNIT_METRICS,
.frsky_vfas_precision = 0,
.frsky_vfas_cell_voltage = 0,
.report_cell_voltage = 0,
.hottAlarmSoundInterval = 5,
.smartportUartUnidirectional = 0,
.smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH,
.ibusTelemetryType = 0,
.ltmUpdateRate = LTM_RATE_NORMAL
.ltmUpdateRate = LTM_RATE_NORMAL,
.halfDuplex = 1,
);
void telemetryInit(void)

6
src/main/telemetry/telemetry.h
View file

@ -24,7 +24,10 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#include "io/serial.h"
@ -58,12 +61,13 @@ typedef struct telemetryConfig_s {
frskyGpsCoordFormat_e frsky_coordinate_format;
frskyUnit_e frsky_unit;
uint8_t frsky_vfas_precision;
uint8_t frsky_vfas_cell_voltage;
uint8_t report_cell_voltage;
uint8_t hottAlarmSoundInterval;
uint8_t smartportUartUnidirectional;
smartportFuelUnit_e smartportFuelUnit;
uint8_t ibusTelemetryType;
uint8_t ltmUpdateRate;
uint8_t halfDuplex;
} telemetryConfig_t;
PG_DECLARE(telemetryConfig_t, telemetryConfig);