Corrected smartport regression issue with msp_shared
Corrected condition where an extra padded zero was added to outgoing msp buffers. Whitespace tidy and removal of unnecessary condition. Commented OSD and CMS from STM32F3DISCOVERY target.h Various improvements to msp_shared (removed byte loops, point arithmetic) Raised schedule priority of CRSF device and MSP calls Reworked buffers in msp_shared to enable writes. Moved msp buffers to msp_shared Added new custom frames and reworking msp implementation Implemented CRSF device info reply Adding crsf ping/pong device info functionality Changed Colibri FC address to Betaflight address Implementing MSP in CRSF Telemetry Decoupled msp functionality from smartport into msp_shared Moved USE_SERVOS to FLASH>128 per mikeller
This commit is contained in:
Curtis Bangert
parent
ab53b178ca
commit
863b24ae6a
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -188,3 +188,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,7 +377,18 @@ 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;
|
||||
processCrsf();
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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