diff --git a/make/source.mk b/make/source.mk index c668bbc113..bc6c3e4913 100644 --- a/make/source.mk +++ b/make/source.mk @@ -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 \ diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 7ef216f3f4..f37cba0ae4 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.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; } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 383ac0ac74..0b547cbc67 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -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); diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 92d883d86e..0832f1b996 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -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 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 0ff719e5d0..cec8f5e4a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -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 // diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 84260118f9..4f0c4165d5 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -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 diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 79d2357cbc..c1deb636b3 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -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(); + } } } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index e5a3e4611e..b183d2927b 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -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); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c new file mode 100644 index 0000000000..699571f3d7 --- /dev/null +++ b/src/main/telemetry/msp_shared.c @@ -0,0 +1,231 @@ +#include +#include +#include +#include +#include + +#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 diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h new file mode 100644 index 0000000000..315c47a615 --- /dev/null +++ b/src/main/telemetry/msp_shared.h @@ -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); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 080df64911..e29e45fe2a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -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; } diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 35f18fc4ae..2c918cb249 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -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); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 304271d8c9..9f9fe12663 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -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(); } diff --git a/src/test/Makefile b/src/test/Makefile index cb5b3a6d05..7a18803e3c 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -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. diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index a98ed6c9e4..09fa725b5f 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -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; } } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc new file mode 100644 index 0000000000..8ef8099531 --- /dev/null +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -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 . + */ + +#include +#include +#include + +#include +#include + +extern "C" { + #include + + #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; + } + +} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 2f92636d8c..0e46af9b92 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -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; } + }