Added support for the FrSky FPort protocol.
This commit is contained in:
mikeller
parent
e1a177a998
commit
3873463fcf
|
|
@ -111,10 +111,12 @@ FC_SRC = \
|
|||
rx/rx_spi.c \
|
||||
rx/crsf.c \
|
||||
rx/sbus.c \
|
||||
rx/sbus_channels.c \
|
||||
rx/spektrum.c \
|
||||
rx/sumd.c \
|
||||
rx/sumh.c \
|
||||
rx/xbus.c \
|
||||
rx/fport.c \
|
||||
sensors/acceleration.c \
|
||||
sensors/boardalignment.c \
|
||||
sensors/compass.c \
|
||||
|
|
@ -222,9 +224,11 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
rx/rx_spi.c \
|
||||
rx/crsf.c \
|
||||
rx/sbus.c \
|
||||
rx/sbus_channels.c \
|
||||
rx/spektrum.c \
|
||||
rx/sumd.c \
|
||||
rx/xbus.c \
|
||||
rx/fport.c \
|
||||
scheduler/scheduler.c \
|
||||
sensors/acceleration.c \
|
||||
sensors/boardalignment.c \
|
||||
|
|
|
|||
|
|
@ -53,5 +53,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"FRSKY_D_RX",
|
||||
"GYRO_RAW",
|
||||
"MAX7456_SIGNAL",
|
||||
"MAX7456_SPICLOCK"
|
||||
"MAX7456_SPICLOCK",
|
||||
"SBUS",
|
||||
"FPORT",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -72,6 +72,8 @@ typedef enum {
|
|||
DEBUG_GYRO_RAW,
|
||||
DEBUG_MAX7456_SIGNAL,
|
||||
DEBUG_MAX7456_SPICLOCK,
|
||||
DEBUG_SBUS,
|
||||
DEBUG_FPORT,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -180,7 +180,9 @@ static const char * const lookupTableSerialRX[] = {
|
|||
"IBUS",
|
||||
"JETIEXBUS",
|
||||
"CRSF",
|
||||
"SRXL"
|
||||
"SRXL",
|
||||
"CUSTOM",
|
||||
"FPORT",
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,366 @@
|
|||
/*
|
||||
* 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_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;
|
||||
|
||||
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 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)
|
||||
{
|
||||
static timeUs_t frameStartAt = 0;
|
||||
static bool frameStarting = false;
|
||||
static bool escapedCharacter = false;
|
||||
static uint8_t bufferPosition = 0;
|
||||
static timeUs_t lastFrameReceivedUs = 0;
|
||||
static bool telemetryFrame = false;
|
||||
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
if (bufferPosition > 0 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
|
||||
reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
|
||||
|
||||
bufferPosition = 0;
|
||||
clearToSend = false;
|
||||
}
|
||||
|
||||
uint8_t val = (uint8_t)c;
|
||||
|
||||
if (val == FPORT_FRAME_MARKER) {
|
||||
frameStarting = true;
|
||||
escapedCharacter = false;
|
||||
frameStartAt = currentTimeUs;
|
||||
|
||||
if (bufferPosition > 0) {
|
||||
uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
|
||||
if (nextWriteIndex != rxBufferReadIndex) {
|
||||
rxBuffer[rxBufferWriteIndex].length = bufferPosition;
|
||||
rxBufferWriteIndex = nextWriteIndex;
|
||||
}
|
||||
bufferPosition = 0;
|
||||
if (telemetryFrame) {
|
||||
clearToSend = true;
|
||||
lastTelemetryFrameReceivedUs = currentTimeUs;
|
||||
}
|
||||
|
||||
if (lastFrameReceivedUs) {
|
||||
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
|
||||
}
|
||||
lastFrameReceivedUs = currentTimeUs;
|
||||
} else {
|
||||
clearToSend = false;
|
||||
}
|
||||
telemetryFrame = false;
|
||||
} else {
|
||||
if ((frameStarting || bufferPosition > 0) && bufferPosition < BUFFER_SIZE) {
|
||||
if (escapedCharacter) {
|
||||
val = val ^ FPORT_ESCAPE_MASK;
|
||||
escapedCharacter = false;
|
||||
} else if (val == FPORT_ESCAPE_CHAR) {
|
||||
escapedCharacter = true;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (bufferPosition == 1 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
|
||||
telemetryFrame = true;
|
||||
}
|
||||
|
||||
frameStarting = false;
|
||||
rxBuffer[rxBufferWriteIndex].data[bufferPosition++] = val;
|
||||
} else if (bufferPosition == BUFFER_SIZE) {
|
||||
bufferPosition = 0;
|
||||
|
||||
reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||
static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
|
||||
{
|
||||
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(void)
|
||||
{
|
||||
static smartPortPayload_t payloadBuffer;
|
||||
static smartPortPayload_t *mspPayload = NULL;
|
||||
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(&frame->data.controlData.channels);
|
||||
|
||||
processRssi(constrain(frame->data.controlData.rssi, 0, 100));
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
mspPayload = NULL;
|
||||
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 defined(USE_TELEMETRY_SMARTPORT)
|
||||
} else {
|
||||
timeUs_t currentTimeUs = micros();
|
||||
if (clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
|
||||
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
|
||||
clearToSend = false;
|
||||
}
|
||||
|
||||
if (clearToSend) {
|
||||
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
|
||||
|
||||
if (hasTelemetryRequest || mspPayload) {
|
||||
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
|
||||
}
|
||||
|
||||
if (clearToSend) {
|
||||
smartPortWriteFrameFport(&emptySmartPortFrame);
|
||||
clearToSend = false;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
sbusChannelsInit(rxConfig);
|
||||
|
||||
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
|
||||
rxRuntimeConfig->rxRefreshRate = 11000;
|
||||
|
||||
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
|
||||
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
fportPort = openSerialPort(portConfig->identifier,
|
||||
FUNCTION_RX_SERIAL,
|
||||
fportDataReceive,
|
||||
FPORT_BAUDRATE,
|
||||
MODE_RXTX,
|
||||
FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0: SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||
);
|
||||
|
||||
if (fportPort) {
|
||||
#if defined(USE_TELEMETRY_SMARTPORT)
|
||||
telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
|
||||
#endif
|
||||
}
|
||||
|
||||
return fportPort != NULL;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -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);
|
||||
|
|
@ -49,6 +49,7 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/pwm.h"
|
||||
#include "rx/fport.h"
|
||||
#include "rx/sbus.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/sumd.h"
|
||||
|
|
@ -275,6 +276,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
|
|||
case SERIALRX_TARGET_CUSTOM:
|
||||
enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_SERIALRX_FPORT
|
||||
case SERIALRX_FPORT:
|
||||
enabled = fportRxInit(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
enabled = false;
|
||||
|
|
|
|||
|
|
@ -18,8 +18,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "common/time.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
#include "drivers/io_types.h"
|
||||
|
||||
#define STICK_CHANNEL_COUNT 4
|
||||
|
||||
#define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
|
||||
|
|
@ -57,6 +60,7 @@ typedef enum {
|
|||
SERIALRX_CRSF = 9,
|
||||
SERIALRX_SRXL = 10,
|
||||
SERIALRX_TARGET_CUSTOM = 11,
|
||||
SERIALRX_FPORT = 12,
|
||||
} SerialRXType;
|
||||
|
||||
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
|
||||
#ifdef USE_SERIAL_RX
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
|
|
@ -32,8 +34,10 @@
|
|||
#ifdef USE_TELEMETRY
|
||||
#include "telemetry/telemetry.h"
|
||||
#endif
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/sbus.h"
|
||||
#include "rx/sbus_channels.h"
|
||||
|
||||
/*
|
||||
* Observations
|
||||
|
|
@ -49,20 +53,10 @@
|
|||
|
||||
#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
|
||||
|
||||
|
|
@ -75,35 +69,19 @@ static uint16_t sbusStateFlags = 0;
|
|||
#define SBUS_DIGITAL_CHANNEL_MIN 173
|
||||
#define SBUS_DIGITAL_CHANNEL_MAX 1812
|
||||
|
||||
enum {
|
||||
DEBUG_SBUS_FRAME_FLAGS = 0,
|
||||
DEBUG_SBUS_STATE_FLAGS,
|
||||
DEBUG_SBUS_FRAME_TIME,
|
||||
};
|
||||
|
||||
static uint16_t sbusStateFlags = 0;
|
||||
|
||||
static bool sbusFrameDone = false;
|
||||
|
||||
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.
|
||||
*
|
||||
|
|
@ -147,9 +125,7 @@ static void sbusDataReceive(uint16_t c)
|
|||
sbusFrameDone = false;
|
||||
} else {
|
||||
sbusFrameDone = true;
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
debug[2] = sbusFrameTime;
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -161,80 +137,31 @@ static uint8_t sbusFrameStatus(void)
|
|||
}
|
||||
sbusFrameDone = false;
|
||||
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
sbusStateFlags = 0;
|
||||
debug[1] = sbusFrame.frame.flags;
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrame.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 (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) {
|
||||
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
|
||||
} else {
|
||||
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
|
||||
}
|
||||
|
||||
if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
if (sbusFrame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
|
||||
sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
|
||||
debug[0] = sbusStateFlags;
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags);
|
||||
}
|
||||
if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
|
||||
// internal failsafe enabled and rx failsafe flag set
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
if (sbusFrame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
|
||||
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;
|
||||
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags);
|
||||
}
|
||||
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
debug[0] = sbusStateFlags;
|
||||
#endif
|
||||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags);
|
||||
|
||||
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 (5 * sbusChannelData[chan] / 8) + 880;
|
||||
return sbusChannelsDecode(&sbusFrame.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;
|
||||
}
|
||||
sbusChannelsInit(rxConfig);
|
||||
|
||||
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
|
||||
rxRuntimeConfig->rxRefreshRate = 11000;
|
||||
|
||||
rxRuntimeConfig->rcReadRawFn = sbusReadRawRC;
|
||||
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
|
||||
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,98 @@
|
|||
/*
|
||||
* 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
|
||||
|
||||
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
|
||||
|
||||
#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(const sbusChannels_t *channels)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
uint16_t sbusChannelsReadRawRC(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 (5 * sbusChannelData[chan] / 8) + 880;
|
||||
}
|
||||
|
||||
void sbusChannelsInit(const rxConfig_t *rxConfig)
|
||||
{
|
||||
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
|
||||
sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* 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(const sbusChannels_t *channels);
|
||||
|
||||
uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
|
||||
|
||||
void sbusChannelsInit(const rxConfig_t *rxConfig);
|
||||
|
||||
|
|
@ -60,10 +60,8 @@
|
|||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
#define USE_UART3
|
||||
#define USE_SOFTSERIAL1
|
||||
#define USE_SOFTSERIAL2
|
||||
|
||||
#define SERIAL_PORT_COUNT 6
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
#define UART1_TX_PIN PA9
|
||||
#define UART1_RX_PIN PA10
|
||||
|
||||
|
|
|
|||
|
|
@ -17,12 +17,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#undef USE_TELEMETRY_IBUS //no space left
|
||||
#undef USE_TELEMETRY_HOTT //no space left
|
||||
#undef USE_TELEMETRY_JETIEXBUS // no space left
|
||||
#undef USE_TELEMETRY_MAVLINK // no space left
|
||||
#undef USE_RCDEVICE // no space left
|
||||
#undef USE_RTC_TIME // no space left
|
||||
// Removed to make the firmware fit into flash:
|
||||
#undef USE_TELEMETRY_IBUS
|
||||
#undef USE_TELEMETRY_HOTT
|
||||
#undef USE_TELEMETRY_JETIEXBUS
|
||||
#undef USE_TELEMETRY_MAVLINK
|
||||
#undef USE_TELEMETRY_LTM
|
||||
#undef USE_RCDEVICE
|
||||
#undef USE_RTC_TIME
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
|
||||
|
||||
|
|
|
|||
|
|
@ -88,8 +88,9 @@
|
|||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
#define USE_UART3
|
||||
#define USE_SOFTSERIAL1
|
||||
#define USE_SOFTSERIAL2
|
||||
// Disabled to make the target fit into flash
|
||||
//#define USE_SOFTSERIAL1
|
||||
//#define USE_SOFTSERIAL2
|
||||
|
||||
#define SOFTSERIAL1_RX_PIN PA6 // PWM 5
|
||||
#define SOFTSERIAL1_TX_PIN PA7 // PWM 6
|
||||
|
|
@ -97,7 +98,7 @@
|
|||
#define SOFTSERIAL2_RX_PIN PB0 // PWM 7
|
||||
#define SOFTSERIAL2_TX_PIN PB1 // PWM 8
|
||||
|
||||
#define SERIAL_PORT_COUNT 6
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
|
||||
#define USE_ESCSERIAL
|
||||
#define ESCSERIAL_TIMER_TX_PIN PA15 // (HARDARE=0,PPM)
|
||||
|
|
|
|||
|
|
@ -95,8 +95,9 @@
|
|||
#define VTX_RTC6705
|
||||
#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
|
||||
|
||||
#undef VTX_SMARTAUDIO // Disabled due to flash size
|
||||
#undef VTX_TRAMP // Disabled due to flash size
|
||||
// Disabled due to flash size
|
||||
#undef VTX_SMARTAUDIO
|
||||
#undef VTX_TRAMP
|
||||
|
||||
#define RTC6705_CS_PIN PF4
|
||||
#define RTC6705_SPI_INSTANCE SPI3
|
||||
|
|
|
|||
|
|
@ -119,6 +119,7 @@
|
|||
#define USE_RX_MSP
|
||||
#define USE_SERIALRX_JETIEXBUS
|
||||
#define USE_SENSOR_NAMES
|
||||
#define USE_SERIALRX_FPORT // FrSky FPort
|
||||
#define USE_VIRTUAL_CURRENT_METER
|
||||
#define VTX_COMMON
|
||||
#define VTX_CONTROL
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
|
@ -60,8 +61,8 @@
|
|||
enum
|
||||
{
|
||||
SPSTATE_UNINITIALIZED,
|
||||
SPSTATE_INITIALIZED,
|
||||
SPSTATE_WORKING
|
||||
SPSTATE_INITIALIZED_SERIAL,
|
||||
SPSTATE_INITIALIZED_EXTERNAL,
|
||||
};
|
||||
|
||||
enum
|
||||
|
|
@ -72,7 +73,8 @@ enum
|
|||
FSSP_DLE_XOR = 0x20,
|
||||
|
||||
FSSP_DATA_FRAME = 0x10,
|
||||
FSSP_MSPC_FRAME = 0x30, // MSP client frame
|
||||
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
|
||||
|
|
@ -145,154 +147,161 @@ const uint16_t frSkyDataIdTable[] = {
|
|||
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 uint8_t smartPortHasRequest = 0;
|
||||
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_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;
|
||||
#if defined(USE_MSP_OVER_TELEMETRY)
|
||||
static bool smartPortMspReplyPending = false;
|
||||
#endif
|
||||
|
||||
static void smartPortDataReceive(uint16_t c)
|
||||
static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToSend)
|
||||
{
|
||||
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 = 0;
|
||||
awaitingSensorId = true;
|
||||
skipUntilStart = false;
|
||||
return;
|
||||
|
||||
return NULL;
|
||||
} else if (skipUntilStart) {
|
||||
return;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer;
|
||||
if (smartPortRxBytes == 0) {
|
||||
if (awaitingSensorId) {
|
||||
awaitingSensorId = false;
|
||||
if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
|
||||
|
||||
// our slot is starting...
|
||||
smartPortLastRequestTime = now;
|
||||
smartPortHasRequest = 1;
|
||||
// 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;
|
||||
} else {
|
||||
skipUntilStart = true;
|
||||
|
||||
checksum += c;
|
||||
checksum = (checksum & 0xFF) + (checksum >> 8);
|
||||
if (checksum == 0xFF) {
|
||||
return (smartPortPayload_t *)&rxBuffer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void smartPortSendByte(uint8_t c, uint16_t *crcp)
|
||||
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);
|
||||
serialWrite(port, FSSP_DLE);
|
||||
serialWrite(port, c ^ FSSP_DLE_XOR);
|
||||
} else {
|
||||
serialWrite(port, c);
|
||||
}
|
||||
|
||||
if (crcp == NULL)
|
||||
return;
|
||||
|
||||
uint16_t crc = *crcp;
|
||||
crc += c;
|
||||
crc += crc >> 8;
|
||||
crc &= 0x00FF;
|
||||
*crcp = crc;
|
||||
if (checksum != NULL) {
|
||||
*checksum += c;
|
||||
}
|
||||
}
|
||||
|
||||
static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data)
|
||||
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);
|
||||
uint8_t *data = (uint8_t *)payload;
|
||||
for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
|
||||
smartPortSendByte(*data++, &checksum, port);
|
||||
}
|
||||
smartPortSendByte(0xFF - (uint8_t)crc, NULL);
|
||||
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);
|
||||
smartPortWriteFrame(&payload);
|
||||
}
|
||||
|
||||
void initSmartPortTelemetry(void)
|
||||
bool initSmartPortTelemetry(void)
|
||||
{
|
||||
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
|
||||
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
|
||||
if (smartPortState == SPSTATE_UNINITIALIZED) {
|
||||
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
|
||||
if (portConfig) {
|
||||
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
|
||||
|
||||
smartPortWriteFrame = smartPortWriteFrameInternal;
|
||||
|
||||
smartPortState = SPSTATE_INITIALIZED_SERIAL;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void freeSmartPortTelemetryPort(void)
|
||||
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
|
||||
{
|
||||
if (smartPortState == SPSTATE_UNINITIALIZED) {
|
||||
smartPortWriteFrame = smartPortWriteFrameExternal;
|
||||
|
||||
smartPortState = SPSTATE_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)
|
||||
{
|
||||
if (!portConfig) {
|
||||
return;
|
||||
|
|
@ -301,82 +310,64 @@ void configureSmartPortTelemetryPort(void)
|
|||
portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
|
||||
|
||||
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, 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 (newTelemetryEnabledValue == smartPortTelemetryEnabled) {
|
||||
return;
|
||||
}
|
||||
if (smartPortState == SPSTATE_INITIALIZED_SERIAL) {
|
||||
bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
|
||||
|
||||
if (newTelemetryEnabledValue)
|
||||
configureSmartPortTelemetryPort();
|
||||
else
|
||||
freeSmartPortTelemetryPort();
|
||||
if (enableSerialTelemetry && !smartPortSerialPort) {
|
||||
configureSmartPortTelemetryPort();
|
||||
} else if (!enableSerialTelemetry && smartPortSerialPort) {
|
||||
freeSmartPortTelemetryPort();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_MSP_OVER_TELEMETRY)
|
||||
void smartPortSendMspResponse(uint8_t *payload) {
|
||||
smartPortSendPackageEx(FSSP_MSPS_FRAME, payload);
|
||||
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 handleSmartPortTelemetry(void)
|
||||
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
|
||||
{
|
||||
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 defined(USE_MSP_OVER_TELEMETRY)
|
||||
if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
|
||||
// Pass only the payload: skip sensorId & frameId
|
||||
uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
|
||||
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_PAYLOAD_SIZE);
|
||||
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 = 0;
|
||||
return;
|
||||
if (requestTimeout) {
|
||||
if (millis() >= *requestTimeout) {
|
||||
*clearToSend = false;
|
||||
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
doRun = false;
|
||||
}
|
||||
|
||||
#if defined(USE_MSP_OVER_TELEMETRY)
|
||||
if (smartPortMspReplyPending) {
|
||||
smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse);
|
||||
smartPortHasRequest = 0;
|
||||
smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
|
||||
*clearToSend = false;
|
||||
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -402,7 +393,7 @@ void handleSmartPortTelemetry(void)
|
|||
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
|
||||
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
|
||||
smartPortSendPackage(id, tmpui);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -415,26 +406,26 @@ void handleSmartPortTelemetry(void)
|
|||
vfasVoltage = getBatteryVoltage();
|
||||
}
|
||||
smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
case FSSP_DATAID_CURRENT :
|
||||
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
|
||||
smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
//case FSSP_DATAID_RPM :
|
||||
case FSSP_DATAID_ALTITUDE :
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
case FSSP_DATAID_FUEL :
|
||||
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
|
||||
smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
//case FSSP_DATAID_ADC1 :
|
||||
|
|
@ -457,7 +448,7 @@ void handleSmartPortTelemetry(void)
|
|||
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
|
||||
}
|
||||
smartPortSendPackage(id, tmpui);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -465,24 +456,24 @@ void handleSmartPortTelemetry(void)
|
|||
case FSSP_DATAID_VARIO :
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
case FSSP_DATAID_HEADING :
|
||||
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
break;
|
||||
case FSSP_DATAID_ACCX :
|
||||
smartPortSendPackage(id, 100 * acc.accSmooth[X] / acc.dev.acc_1G); // Multiply by 100 to show as x.xx g on Taranis
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
break;
|
||||
case FSSP_DATAID_ACCY :
|
||||
smartPortSendPackage(id, 100 * acc.accSmooth[Y] / acc.dev.acc_1G);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
break;
|
||||
case FSSP_DATAID_ACCZ :
|
||||
smartPortSendPackage(id, 100 * acc.accSmooth[Z] / acc.dev.acc_1G);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
break;
|
||||
case FSSP_DATAID_T1 :
|
||||
// we send all the flags as decimal digits for easy reading
|
||||
|
|
@ -528,18 +519,18 @@ void handleSmartPortTelemetry(void)
|
|||
tmpi += 4000;
|
||||
|
||||
smartPortSendPackage(id, (uint32_t)tmpi);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
break;
|
||||
case FSSP_DATAID_T2 :
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
#ifdef USE_GPS
|
||||
// provide GPS lock status
|
||||
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
#endif
|
||||
} else if (feature(FEATURE_GPS)) {
|
||||
smartPortSendPackage(id, 0);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
} else if (telemetryConfig()->pidValuesAsTelemetry) {
|
||||
switch (t2Cnt) {
|
||||
case 0:
|
||||
|
|
@ -569,28 +560,47 @@ void handleSmartPortTelemetry(void)
|
|||
t2Cnt = 0;
|
||||
}
|
||||
smartPortSendPackage(id, tmp2);
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
#ifdef USE_GPS
|
||||
case FSSP_DATAID_GPS_ALT :
|
||||
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
||||
smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
case FSSP_DATAID_A4 :
|
||||
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
|
||||
smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts
|
||||
smartPortHasRequest = 0;
|
||||
*clearToSend = false;
|
||||
}
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void handleSmartPortTelemetry(void)
|
||||
{
|
||||
static bool clearToSend = false;
|
||||
|
||||
const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
|
||||
|
||||
if (!(smartPortState == SPSTATE_INITIALIZED_SERIAL && smartPortSerialPort)) {
|
||||
return;
|
||||
}
|
||||
|
||||
smartPortPayload_t *payload = NULL;
|
||||
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
|
||||
uint8_t c = serialRead(smartPortSerialPort);
|
||||
payload = smartPortDataReceiveSerial(c, &clearToSend);
|
||||
}
|
||||
|
||||
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,14 +7,25 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#define SMARTPORT_MSP_TX_BUF_SIZE 256
|
||||
#define SMARTPORT_MSP_RX_BUF_SIZE 64
|
||||
|
||||
void initSmartPortTelemetry(void);
|
||||
typedef struct smartPortPayload_s {
|
||||
uint8_t frameId;
|
||||
uint16_t valueId;
|
||||
uint32_t data;
|
||||
} __attribute__((packed)) smartPortPayload_t;
|
||||
|
||||
typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
|
||||
|
||||
bool initSmartPortTelemetry(void);
|
||||
void checkSmartPortTelemetryState(void);
|
||||
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal);
|
||||
|
||||
void handleSmartPortTelemetry(void);
|
||||
void checkSmartPortTelemetryState(void);
|
||||
|
||||
void configureSmartPortTelemetryPort(void);
|
||||
void freeSmartPortTelemetryPort(void);
|
||||
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout);
|
||||
|
||||
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum);
|
||||
void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port);
|
||||
|
|
|
|||
Loading…
Reference in New Issue