Merge pull request #4050 from codecae/msp_shared_crsf_smartport
Decoupled MSP functionality from S.Port. Implemented MSP telemetry in CRSF.
This commit is contained in:
commit
6bf755d0bd
|
@ -159,6 +159,7 @@ FC_SRC = \
|
|||
telemetry/smartport.c \
|
||||
telemetry/ltm.c \
|
||||
telemetry/mavlink.c \
|
||||
telemetry/msp_shared.c \
|
||||
telemetry/ibus.c \
|
||||
telemetry/ibus_shared.c \
|
||||
sensors/esc_sensor.c \
|
||||
|
|
|
@ -33,22 +33,29 @@
|
|||
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_uart.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/crsf.h"
|
||||
|
||||
#include "telemetry/crsf.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
#define CRSF_TIME_NEEDED_PER_FRAME_US 1000
|
||||
#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds
|
||||
|
||||
#define CRSF_DIGITAL_CHANNEL_MIN 172
|
||||
#define CRSF_DIGITAL_CHANNEL_MAX 1811
|
||||
|
||||
#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
|
||||
|
||||
STATIC_UNIT_TESTED bool crsfFrameDone = false;
|
||||
STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
|
||||
|
||||
STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
|
||||
|
||||
static serialPort_t *serialPort;
|
||||
|
@ -56,7 +63,6 @@ static uint32_t crsfFrameStartAt = 0;
|
|||
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
|
||||
static uint8_t telemetryBufLen = 0;
|
||||
|
||||
|
||||
/*
|
||||
* CRSF protocol
|
||||
*
|
||||
|
@ -133,6 +139,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
|
|||
if (crsfFramePosition < fullFrameLength) {
|
||||
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
|
||||
crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
|
||||
if (crsfFrameDone) {
|
||||
crsfFramePosition = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -176,6 +185,20 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
|
|||
crsfChannelData[14] = rcChannels->chan14;
|
||||
crsfChannelData[15] = rcChannels->chan15;
|
||||
return RX_FRAME_COMPLETE;
|
||||
} else {
|
||||
if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
|
||||
// TODO: CRC CHECK
|
||||
scheduleDeviceInfoResponse();
|
||||
return RX_FRAME_COMPLETE;
|
||||
} else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) {
|
||||
// TODO: CRC CHECK
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
if(handleMspFrame(frameStart, frameEnd)) {
|
||||
scheduleMspResponse();
|
||||
}
|
||||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
}
|
||||
}
|
||||
return RX_FRAME_PENDING;
|
||||
|
@ -222,6 +245,7 @@ void crsfRxSendTelemetryData(void)
|
|||
|
||||
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
|
||||
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
|
||||
crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
|
||||
}
|
||||
|
|
|
@ -22,6 +22,8 @@
|
|||
#define CRSF_PORT_MODE MODE_RXTX
|
||||
|
||||
#define CRSF_MAX_CHANNEL 16
|
||||
#define CRSF_MSP_RX_BUF_SIZE 128
|
||||
#define CRSF_MSP_TX_BUF_SIZE 128
|
||||
|
||||
typedef enum {
|
||||
CRSF_FRAMETYPE_GPS = 0x02,
|
||||
|
@ -29,7 +31,12 @@ typedef enum {
|
|||
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
|
||||
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
|
||||
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
|
||||
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
|
||||
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
|
||||
CRSF_FRAMETYPE_DEVICE_PING = 0x28,
|
||||
CRSF_FRAMETYPE_DEVICE_INFO = 0x29,
|
||||
CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command
|
||||
CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary
|
||||
CRSF_FRAMETYPE_MSP_WRITE = 0x7C // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
|
||||
} crsfFrameType_e;
|
||||
|
||||
enum {
|
||||
|
@ -38,11 +45,14 @@ enum {
|
|||
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
|
||||
CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
|
||||
CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
|
||||
CRSF_FRAME_TX_MSP_PAYLOAD_SIZE = 58,
|
||||
CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8,
|
||||
CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
|
||||
CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
|
||||
CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
|
||||
CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field
|
||||
CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
|
||||
CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined
|
||||
CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4 // length of Extended Dest/Origin, TYPE and CRC fields combined
|
||||
};
|
||||
|
||||
enum {
|
||||
|
@ -51,7 +61,7 @@ enum {
|
|||
CRSF_ADDRESS_RESERVED1 = 0x8A,
|
||||
CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
|
||||
CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
|
||||
CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
|
||||
CRSF_ADDRESS_BETAFLIGHT = 0xC8,
|
||||
CRSF_ADDRESS_RESERVED2 = 0xCA,
|
||||
CRSF_ADDRESS_RACE_TAG = 0xCC,
|
||||
CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
|
||||
|
@ -59,7 +69,7 @@ enum {
|
|||
CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
|
||||
};
|
||||
|
||||
#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking
|
||||
#define CRSF_PAYLOAD_SIZE_MAX 60 // Size confirmed by Remo
|
||||
#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4)
|
||||
|
||||
typedef struct crsfFrameDef_s {
|
||||
|
@ -74,7 +84,6 @@ typedef union crsfFrame_u {
|
|||
crsfFrameDef_t frame;
|
||||
} crsfFrame_t;
|
||||
|
||||
|
||||
void crsfRxWriteTelemetryData(const void *data, int len);
|
||||
void crsfRxSendTelemetryData(void);
|
||||
|
||||
|
|
|
@ -189,3 +189,5 @@
|
|||
#else
|
||||
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16))
|
||||
#endif
|
||||
|
||||
#undef USE_DASHBOARD
|
||||
|
|
|
@ -129,12 +129,12 @@
|
|||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_MS5611
|
||||
|
||||
#define OSD
|
||||
//#define OSD
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI2
|
||||
#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN
|
||||
|
||||
#define CMS
|
||||
//#define CMS
|
||||
|
||||
//#define USE_SDCARD
|
||||
//
|
||||
|
|
|
@ -102,7 +102,6 @@
|
|||
#define TELEMETRY_LTM
|
||||
#define TELEMETRY_SMARTPORT
|
||||
#define USE_RESOURCE_MGMT
|
||||
#define USE_SERVOS
|
||||
#endif
|
||||
|
||||
#if (FLASH_SIZE > 128)
|
||||
|
@ -118,6 +117,7 @@
|
|||
#define USE_RX_MSP
|
||||
#define USE_SERIALRX_JETIEXBUS
|
||||
#define USE_SENSOR_NAMES
|
||||
#define USE_SERVOS
|
||||
#define USE_VIRTUAL_CURRENT_METER
|
||||
#define VTX_COMMON
|
||||
#define VTX_CONTROL
|
||||
|
|
|
@ -24,6 +24,7 @@
|
|||
#ifdef TELEMETRY
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "build/build_config.h"
|
||||
#include "build/version.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
|
@ -31,6 +32,7 @@
|
|||
|
||||
#include "common/crc.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
|
@ -51,12 +53,17 @@
|
|||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/crsf.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
|
||||
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
|
||||
#define CRSF_DEVICEINFO_VERSION 0x01
|
||||
#define CRSF_DEVICEINFO_PARAMETER_COUNT 255
|
||||
|
||||
static bool crsfTelemetryEnabled;
|
||||
static bool deviceInfoReplyPending;
|
||||
static bool mspReplyPending;
|
||||
static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
|
||||
|
||||
static void crsfInitializeFrame(sbuf_t *dst)
|
||||
|
@ -223,6 +230,41 @@ void crsfFrameFlightMode(sbuf_t *dst)
|
|||
*lengthPtr = sbufPtr(dst) - lengthPtr;
|
||||
}
|
||||
|
||||
void scheduleDeviceInfoResponse() {
|
||||
deviceInfoReplyPending = true;
|
||||
}
|
||||
|
||||
/*
|
||||
0x29 Device Info
|
||||
Payload:
|
||||
uint8_t Destination
|
||||
uint8_t Origin
|
||||
char[] Device Name ( Null terminated string )
|
||||
uint32_t Null Bytes
|
||||
uint32_t Null Bytes
|
||||
uint32_t Null Bytes
|
||||
uint8_t 255 (Max MSP Parameter)
|
||||
uint8_t 0x01 (Parameter version 1)
|
||||
*/
|
||||
void crsfFrameDeviceInfo(sbuf_t *dst) {
|
||||
|
||||
char buff[30];
|
||||
tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
|
||||
|
||||
uint8_t *lengthPtr = sbufPtr(dst);
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
|
||||
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
|
||||
sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT);
|
||||
sbufWriteStringWithZeroTerminator(dst, buff);
|
||||
for (unsigned int ii=0; ii<12; ii++) {
|
||||
sbufWriteU8(dst, 0x00);
|
||||
}
|
||||
sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
|
||||
sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
|
||||
*lengthPtr = sbufPtr(dst) - lengthPtr;
|
||||
}
|
||||
|
||||
#define BV(x) (1 << (x)) // bit value
|
||||
|
||||
// schedule array to decide how often each type of frame is sent
|
||||
|
@ -238,6 +280,25 @@ typedef enum {
|
|||
static uint8_t crsfScheduleCount;
|
||||
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
|
||||
|
||||
void scheduleMspResponse() {
|
||||
if (!mspReplyPending) {
|
||||
mspReplyPending = true;
|
||||
}
|
||||
}
|
||||
|
||||
void crsfSendMspResponse(uint8_t *payload)
|
||||
{
|
||||
sbuf_t crsfPayloadBuf;
|
||||
sbuf_t *dst = &crsfPayloadBuf;
|
||||
|
||||
crsfInitializeFrame(dst);
|
||||
sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
|
||||
sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
|
||||
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
|
||||
sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT);
|
||||
sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
|
||||
static void processCrsf(void)
|
||||
{
|
||||
|
@ -257,6 +318,7 @@ static void processCrsf(void)
|
|||
crsfFrameBatterySensor(dst);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
|
||||
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameFlightMode(dst);
|
||||
|
@ -277,6 +339,10 @@ void initCrsfTelemetry(void)
|
|||
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
|
||||
// and feature is enabled, if so, set CRSF telemetry enabled
|
||||
crsfTelemetryEnabled = crsfRxIsActive();
|
||||
|
||||
deviceInfoReplyPending = false;
|
||||
mspReplyPending = false;
|
||||
|
||||
int index = 0;
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
|
||||
|
@ -311,8 +377,19 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
|
|||
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
|
||||
if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
|
||||
crsfLastCycleTime = currentTimeUs;
|
||||
if (deviceInfoReplyPending) {
|
||||
sbuf_t crsfPayloadBuf;
|
||||
sbuf_t *dst = &crsfPayloadBuf;
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameDeviceInfo(dst);
|
||||
crsfFinalize(dst);
|
||||
deviceInfoReplyPending = false;
|
||||
} else if (mspReplyPending) {
|
||||
mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse);
|
||||
} else {
|
||||
processCrsf();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
|
||||
|
|
|
@ -19,9 +19,12 @@
|
|||
|
||||
#include "common/time.h"
|
||||
#include "rx/crsf.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
void initCrsfTelemetry(void);
|
||||
bool checkCrsfTelemetryState(void);
|
||||
void handleCrsfTelemetry(timeUs_t currentTimeUs);
|
||||
void scheduleDeviceInfoResponse();
|
||||
void scheduleMspResponse();
|
||||
|
||||
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
|
||||
|
|
|
@ -0,0 +1,231 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef TELEMETRY
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "fc/fc_msp.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "rx/crsf.h"
|
||||
#include "rx/msp.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
|
||||
};
|
||||
|
||||
#define REQUEST_BUFFER_SIZE 64
|
||||
#define RESPONSE_BUFFER_SIZE 64
|
||||
|
||||
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() {
|
||||
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()
|
||||
{
|
||||
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, uint8_t *frameEnd)
|
||||
{
|
||||
static uint8_t mspStarted = 0;
|
||||
static uint8_t lastSeq = 0;
|
||||
|
||||
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (mspStarted == 0) {
|
||||
initSharedMsp();
|
||||
}
|
||||
|
||||
mspPacket_t *packet = mspPackage.requestPacket;
|
||||
sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd);
|
||||
sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
|
||||
uint8_t header = sbufReadU8(frameBuf);
|
||||
uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
|
||||
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;
|
||||
}
|
||||
|
||||
uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
|
||||
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));
|
||||
}
|
||||
|
||||
uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
|
||||
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
|
|
@ -0,0 +1,29 @@
|
|||
#pragma once
|
||||
|
||||
#include "msp/msp.h"
|
||||
#include "rx/crsf.h"
|
||||
#include "telemetry/smartport.h"
|
||||
|
||||
typedef void (*mspResponseFnPtr)(uint8_t *payload);
|
||||
|
||||
typedef struct mspPackage_s {
|
||||
sbuf_t requestFrame;
|
||||
uint8_t *requestBuffer;
|
||||
uint8_t *responseBuffer;
|
||||
mspPacket_t *requestPacket;
|
||||
mspPacket_t *responsePacket;
|
||||
} mspPackage_t;
|
||||
|
||||
typedef union mspRxBuffer_u {
|
||||
uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
|
||||
uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
|
||||
} mspRxBuffer_t;
|
||||
|
||||
typedef union mspTxBuffer_u {
|
||||
uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
|
||||
uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
|
||||
} mspTxBuffer_t;
|
||||
|
||||
void initSharedMsp();
|
||||
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
|
||||
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
|
|
@ -43,8 +43,6 @@
|
|||
#include "io/gps.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/battery.h"
|
||||
|
@ -54,10 +52,10 @@
|
|||
#include "sensors/gyro.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/smartport.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
enum
|
||||
{
|
||||
|
@ -164,7 +162,6 @@ typedef struct smartPortFrame_s {
|
|||
} __attribute__((packed)) smartPortFrame_t;
|
||||
|
||||
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
|
||||
#define SMARTPORT_TX_BUF_SIZE 256
|
||||
|
||||
#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
|
||||
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
|
||||
|
@ -172,30 +169,8 @@ typedef struct smartPortFrame_s {
|
|||
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;
|
||||
static bool smartPortMspReplyPending = false;
|
||||
|
||||
#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)
|
||||
{
|
||||
static bool skipUntilStart = true;
|
||||
|
@ -352,194 +327,8 @@ void checkSmartPortTelemetryState(void)
|
|||
freeSmartPortTelemetryPort();
|
||||
}
|
||||
|
||||
static void initSmartPortMspReply(int16_t cmd)
|
||||
{
|
||||
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()
|
||||
{
|
||||
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) {
|
||||
// no start packet yet, throw this one away
|
||||
return;
|
||||
} 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 smartPortSendMspResponse(uint8_t *payload) {
|
||||
smartPortSendPackageEx(FSSP_MSPS_FRAME, payload);
|
||||
}
|
||||
|
||||
void handleSmartPortTelemetry(void)
|
||||
|
@ -566,7 +355,11 @@ void handleSmartPortTelemetry(void)
|
|||
if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
|
||||
|
||||
// Pass only the payload: skip sensorId & frameId
|
||||
handleSmartPortMspFrame(&smartPortRxBuffer);
|
||||
if (!smartPortMspReplyPending) {
|
||||
uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
|
||||
uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
|
||||
smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -578,7 +371,7 @@ void handleSmartPortTelemetry(void)
|
|||
}
|
||||
|
||||
if (smartPortMspReplyPending) {
|
||||
smartPortMspReplyPending = smartPortSendMspReply();
|
||||
smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse);
|
||||
smartPortHasRequest = 0;
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -7,6 +7,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define SMARTPORT_MSP_TX_BUF_SIZE 256
|
||||
#define SMARTPORT_MSP_RX_BUF_SIZE 64
|
||||
|
||||
void initSmartPortTelemetry(void);
|
||||
|
||||
void handleSmartPortTelemetry(void);
|
||||
|
|
|
@ -52,6 +52,7 @@
|
|||
#include "telemetry/crsf.h"
|
||||
#include "telemetry/srxl.h"
|
||||
#include "telemetry/ibus.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
|
||||
|
@ -100,6 +101,7 @@ void telemetryInit(void)
|
|||
#ifdef TELEMETRY_IBUS
|
||||
initIbusTelemetry();
|
||||
#endif
|
||||
initSharedMsp();
|
||||
|
||||
telemetryCheckState();
|
||||
}
|
||||
|
|
|
@ -172,7 +172,10 @@ rc_controls_unittest_SRC := \
|
|||
rx_crsf_unittest_SRC := \
|
||||
$(USER_DIR)/rx/crsf.c \
|
||||
$(USER_DIR)/common/crc.c \
|
||||
$(USER_DIR)/common/streambuf.c
|
||||
$(USER_DIR)/common/printf.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/drivers/serial.c
|
||||
|
||||
|
||||
rx_ibus_unittest_SRC := \
|
||||
|
@ -207,8 +210,24 @@ telemetry_crsf_unittest_SRC := \
|
|||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/common/gps_conversion.c \
|
||||
$(USER_DIR)/common/printf.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/fc/runtime_config.c
|
||||
|
||||
telemetry_crsf_msp_unittest_SRC := \
|
||||
$(USER_DIR)/rx/crsf.c \
|
||||
$(USER_DIR)/common/crc.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/common/printf.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/drivers/serial.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/telemetry/crsf.c \
|
||||
$(USER_DIR)/common/gps_conversion.c \
|
||||
$(USER_DIR)/telemetry/msp_shared.c \
|
||||
$(USER_DIR)/fc/runtime_config.c
|
||||
|
||||
|
||||
telemetry_crsf_unittest_DEFINES := \
|
||||
FLASH_SIZE=128 \
|
||||
STM32F10X_MD \
|
||||
|
@ -254,6 +273,9 @@ huffman_unittest_SRC := \
|
|||
huffman_unittest_DEFINES := \
|
||||
USE_HUFFMAN
|
||||
|
||||
ringbuffer_unittest_SRC := \
|
||||
$(USER_DIR)/common/ringbuffer.c
|
||||
|
||||
# Please tweak the following variable definitions as needed by your
|
||||
# project, except GTEST_HEADERS, which you can use in your own targets
|
||||
# but shouldn't modify.
|
||||
|
|
|
@ -29,11 +29,14 @@ extern "C" {
|
|||
#include "common/crc.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/serial.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/crsf.h"
|
||||
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
void crsfDataReceive(uint16_t c);
|
||||
uint8_t crsfFrameCRC(void);
|
||||
uint8_t crsfFrameStatus(void);
|
||||
|
@ -277,7 +280,9 @@ int16_t debug[DEBUG16_VALUE_COUNT];
|
|||
uint32_t micros(void) {return dummyTimeUs;}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
|
||||
void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
|
||||
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
|
||||
serialPort_t *telemetrySharedPort = NULL;
|
||||
void scheduleDeviceInfoResponse(void) {};
|
||||
void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
|
||||
bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
|
||||
}
|
||||
|
|
|
@ -0,0 +1,299 @@
|
|||
/*
|
||||
* 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 <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <limits.h>
|
||||
#include <algorithm>
|
||||
|
||||
extern "C" {
|
||||
#include <platform.h>
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/crc.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/gps_conversion.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/typeconversion.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/config.h"
|
||||
#include "flight/imu.h"
|
||||
#include "fc/fc_msp.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/crsf.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
#include "telemetry/smartport.h"
|
||||
|
||||
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
|
||||
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
|
||||
uint8_t sbufReadU8(sbuf_t *src);
|
||||
int sbufBytesRemaining(sbuf_t *buf);
|
||||
void initSharedMsp();
|
||||
uint16_t testBatteryVoltage = 0;
|
||||
int32_t testAmperage = 0;
|
||||
uint8_t mspTxData[64]; //max frame size
|
||||
sbuf_t mspTxDataBuf;
|
||||
uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX];
|
||||
uint8_t payloadOutput[64];
|
||||
sbuf_t payloadOutputBuf;
|
||||
int32_t testmAhDrawn = 0;
|
||||
|
||||
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
|
||||
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
|
||||
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
|
||||
|
||||
extern bool crsfFrameDone;
|
||||
extern crsfFrame_t crsfFrame;
|
||||
extern mspPackage_t mspPackage;
|
||||
extern uint8_t checksum;
|
||||
|
||||
uint32_t dummyTimeUs;
|
||||
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
typedef struct crsfMspFrame_s {
|
||||
uint8_t deviceAddress;
|
||||
uint8_t frameLength;
|
||||
uint8_t type;
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
|
||||
} crsfMspFrame_t;
|
||||
|
||||
const uint8_t crsfPidRequest[] = {
|
||||
0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
|
||||
};
|
||||
|
||||
TEST(CrossFireMSPTest, RequestBufferTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type);
|
||||
uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload;
|
||||
uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
EXPECT_EQ(0xC8, *destination);
|
||||
EXPECT_EQ(0xEA, *origin);
|
||||
EXPECT_EQ(0x30, *frameStart);
|
||||
EXPECT_EQ(0x69, *frameEnd);
|
||||
}
|
||||
|
||||
TEST(CrossFireMSPTest, ResponsePacketTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
handleMspFrame(frameStart, frameEnd);
|
||||
for (unsigned int ii=1; ii<30; ii++) {
|
||||
EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
|
||||
}
|
||||
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
|
||||
}
|
||||
|
||||
const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32};
|
||||
const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00};
|
||||
const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19};
|
||||
const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00};
|
||||
const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00};
|
||||
|
||||
TEST(CrossFireMSPTest, WriteResponseTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr1;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool pending1 = handleMspFrame(frameStart, frameEnd);
|
||||
EXPECT_FALSE(pending1); // not done yet*/
|
||||
EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
|
||||
EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
|
||||
EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]);
|
||||
EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[4]);
|
||||
|
||||
const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr2;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool pending2 = handleMspFrame(frameStart2, frameEnd2);
|
||||
EXPECT_FALSE(pending2); // not done yet
|
||||
EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
|
||||
EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
|
||||
EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]);
|
||||
EXPECT_EQ(0x14, mspPackage.requestBuffer[8]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[9]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[10]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[11]);
|
||||
|
||||
const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr3;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending3 = handleMspFrame(frameStart3, frameEnd3);
|
||||
EXPECT_FALSE(pending3); // not done yet
|
||||
EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[14]);
|
||||
EXPECT_EQ(0x22, mspPackage.requestBuffer[15]);
|
||||
EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]);
|
||||
EXPECT_EQ(0x35, mspPackage.requestBuffer[17]);
|
||||
EXPECT_EQ(0x19, mspPackage.requestBuffer[18]);
|
||||
|
||||
const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr4;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending4 = handleMspFrame(frameStart4, frameEnd4);
|
||||
EXPECT_FALSE(pending4); // not done yet
|
||||
EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
|
||||
EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[21]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[22]);
|
||||
EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]);
|
||||
EXPECT_EQ(0x28, mspPackage.requestBuffer[24]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[25]);
|
||||
//EXPECT_EQ(0xB3,checksum);
|
||||
|
||||
const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr5;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending5 = handleMspFrame(frameStart5, frameEnd5);
|
||||
EXPECT_TRUE(pending5); // not done yet
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
|
||||
EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
|
||||
EXPECT_EQ(0x37, mspPackage.requestBuffer[28]);
|
||||
EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]);
|
||||
EXPECT_EQ(0xF8,checksum);
|
||||
|
||||
}
|
||||
|
||||
void testSendMspResponse(uint8_t *payload) {
|
||||
sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
|
||||
sbufWriteData(plOut, payload, *payload + 64);
|
||||
sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
|
||||
}
|
||||
|
||||
TEST(CrossFireMSPTest, SendMspReply) {
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool handled = handleMspFrame(frameStart, frameEnd);
|
||||
EXPECT_TRUE(handled);
|
||||
bool replyPending = sendMspReply(64, &testSendMspResponse);
|
||||
EXPECT_FALSE(replyPending);
|
||||
EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf));
|
||||
EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf));
|
||||
for (unsigned int ii=1; ii<=30; ii++) {
|
||||
EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
|
||||
}
|
||||
EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
||||
gpsSolutionData_t gpsSol;
|
||||
attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
|
||||
|
||||
uint32_t micros(void) {return dummyTimeUs;}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
|
||||
uint16_t getBatteryVoltage(void) {
|
||||
return testBatteryVoltage;
|
||||
}
|
||||
int32_t getAmperage(void) {
|
||||
return testAmperage;
|
||||
}
|
||||
|
||||
uint8_t calculateBatteryPercentageRemaining(void) {
|
||||
return 67;
|
||||
}
|
||||
|
||||
bool feature(uint32_t) {return false;}
|
||||
|
||||
bool isAirmodeActive(void) {return true;}
|
||||
|
||||
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
|
||||
|
||||
UNUSED(mspPostProcessFn);
|
||||
|
||||
sbuf_t *dst = &reply->buf;
|
||||
//sbuf_t *src = &cmd->buf;
|
||||
const uint8_t cmdMSP = cmd->cmd;
|
||||
reply->cmd = cmd->cmd;
|
||||
|
||||
if (cmdMSP == 0x70) {
|
||||
for (unsigned int ii=1; ii<=30; ii++) {
|
||||
sbufWriteU8(dst, ii);
|
||||
}
|
||||
} else if (cmdMSP == 0xCA) {
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
void beeperConfirmationBeeps(uint8_t ) {}
|
||||
|
||||
int32_t getMAhDrawn(void) {
|
||||
return testmAhDrawn;
|
||||
}
|
||||
|
||||
}
|
|
@ -31,6 +31,8 @@ extern "C" {
|
|||
#include "common/filter.h"
|
||||
#include "common/gps_conversion.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/typeconversion.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
@ -38,6 +40,7 @@ extern "C" {
|
|||
#include "drivers/serial.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
@ -53,6 +56,7 @@ extern "C" {
|
|||
|
||||
#include "telemetry/crsf.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
bool airMode;
|
||||
|
||||
|
@ -63,6 +67,7 @@ extern "C" {
|
|||
serialPort_t *telemetrySharedPort;
|
||||
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
|
||||
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
|
||||
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
@ -293,6 +298,7 @@ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
|
|||
void serialSetMode(serialPort_t *, portMode_e) {}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
|
||||
void closeSerialPort(serialPort_t *) {}
|
||||
bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; }
|
||||
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
|
||||
|
||||
|
@ -323,4 +329,7 @@ int32_t getMAhDrawn(void){
|
|||
return testmAhDrawn;
|
||||
}
|
||||
|
||||
bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
|
||||
bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue