mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 17:55:28 +03:00
Add support for sharing DEBUG_TRACE port with an MSP port
- Added MSPv2 code MSP2_COMMON_DEBUG_OUTPUT, which sends a debug message to the MSP client. It must be ready to accept MSP responses without sending a request first. - Moved MSP initialization a bit early in the boot process, so we can print debug output during HW initialization. - Added a couple of functions in msp_serial.c to locate an MSP port from a serial port and to push a message to a given MSP port. - Updated isSerialConfigValid() to allow sharing MSP with DEBUG_TRACE.
This commit is contained in:
parent
8e77390fb2
commit
638c9de7a8
6 changed files with 84 additions and 18 deletions
|
@ -27,6 +27,7 @@
|
|||
#include "drivers/time.h"
|
||||
|
||||
#include "common/printf.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
|
@ -34,6 +35,10 @@
|
|||
|
||||
#include "fc/config.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
#include "msp/msp_serial.h"
|
||||
#include "msp/msp_protocol_v2_common.h"
|
||||
|
||||
#ifdef DEBUG_SECTION_TIMES
|
||||
timeUs_t sectionTimes[2][4];
|
||||
#endif
|
||||
|
@ -43,6 +48,7 @@ uint8_t debugMode;
|
|||
|
||||
#if defined(USE_DEBUG_TRACE)
|
||||
static serialPort_t * tracePort = NULL;
|
||||
static mspPort_t * mspTracePort = NULL;
|
||||
|
||||
void debugTraceInit(void)
|
||||
{
|
||||
|
@ -55,8 +61,35 @@ void debugTraceInit(void)
|
|||
return;
|
||||
}
|
||||
|
||||
tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
|
||||
bool tracePortIsSharedWithMSP = false;
|
||||
|
||||
if (determinePortSharing(portConfig, FUNCTION_DEBUG_TRACE) == PORTSHARING_SHARED) {
|
||||
// We support sharing a DEBUG_TRACE port only with MSP
|
||||
if (portConfig->functionMask != (FUNCTION_DEBUG_TRACE | FUNCTION_MSP)) {
|
||||
return;
|
||||
}
|
||||
tracePortIsSharedWithMSP = true;
|
||||
}
|
||||
|
||||
// If the port is shared with MSP, reuse the port
|
||||
if (tracePortIsSharedWithMSP) {
|
||||
const serialPort_t *traceAndMspPort = findSharedSerialPort(FUNCTION_DEBUG_TRACE, FUNCTION_MSP);
|
||||
if (!traceAndMspPort) {
|
||||
return;
|
||||
}
|
||||
|
||||
mspTracePort = mspSerialPortFind(traceAndMspPort);
|
||||
if (!mspTracePort) {
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
|
||||
if (!tracePort) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
// Initialization done
|
||||
DEBUG_TRACE_SYNC("%s/%s %s %s / %s (%s)",
|
||||
FC_FIRMWARE_NAME,
|
||||
targetName,
|
||||
|
@ -103,10 +136,20 @@ void debugTracePrintf(bool synchronous, const char *format, ...)
|
|||
waitForSerialPortToFinishTransmitting(tracePort);
|
||||
}
|
||||
}
|
||||
} else if (mspTracePort) {
|
||||
// We might need to split the message into several chunks if it's
|
||||
// longer than MSP_PORT_PUSH_BUFFER_SIZE
|
||||
int sent = 0;
|
||||
while (sent < charCount) {
|
||||
int count = MIN(charCount - sent, MSP_PORT_PUSH_BUFFER_SIZE);
|
||||
mspSerialPushPort(MSP2_COMMON_DEBUG_OUTPUT, (uint8_t*)(buf + sent), count, mspTracePort, MSP_V2_NATIVE);
|
||||
sent += count;
|
||||
}
|
||||
else {
|
||||
// Send data via MSPV2_TRACE message
|
||||
// TODO
|
||||
// Finally, send a '\0' to indicate the message is complete. This should
|
||||
// be easy to detect in JS and it allows C clients to reuse the buffer since
|
||||
// it's now zero terminated.
|
||||
char end = '\0';
|
||||
mspSerialPushPort(MSP2_COMMON_DEBUG_OUTPUT, (uint8_t*)&end, sizeof(end), mspTracePort, MSP_V2_NATIVE);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -255,6 +255,10 @@ void init(void)
|
|||
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
|
||||
#endif
|
||||
|
||||
// Initialize MSP here so the DEBUG_TRACE can share a port with MSP
|
||||
mspFcInit();
|
||||
mspSerialInit();
|
||||
|
||||
#if defined(USE_DEBUG_TRACE)
|
||||
// Debug trace uses serial output, so we only can init it after serial port is ready
|
||||
// From this point on we can use DEBUG_TRACE() to produce real-time debugging information
|
||||
|
@ -550,9 +554,6 @@ void init(void)
|
|||
|
||||
imuInit();
|
||||
|
||||
mspFcInit();
|
||||
mspSerialInit();
|
||||
|
||||
#ifdef USE_CLI
|
||||
cliInit(serialConfig());
|
||||
#endif
|
||||
|
|
|
@ -256,7 +256,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
|
|||
}
|
||||
|
||||
#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
|
||||
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK)
|
||||
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_DEBUG_TRACE)
|
||||
|
||||
bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
|
||||
{
|
||||
|
@ -264,7 +264,7 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
|
|||
/*
|
||||
* rules:
|
||||
* - 1 MSP port minimum, max MSP ports is defined and must be adhered to.
|
||||
* - MSP is allowed to be shared with EITHER any telemetry OR blackbox.
|
||||
* - MSP is allowed to be shared with EITHER any telemetry OR blackbox OR debug trace.
|
||||
* - serial RX and FrSky / LTM telemetry can be shared
|
||||
* - No other sharing combinations are valid.
|
||||
*/
|
||||
|
|
|
@ -22,3 +22,5 @@
|
|||
|
||||
#define MSP2_COMMON_MOTOR_MIXER 0x1005
|
||||
#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006
|
||||
|
||||
#define MSP2_COMMON_DEBUG_OUTPUT 0x1007 //out message Prints debug data. Receiver must be ready to accept responses without requests
|
||||
|
|
|
@ -497,10 +497,9 @@ void mspSerialInit(void)
|
|||
mspSerialAllocatePorts();
|
||||
}
|
||||
|
||||
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
|
||||
int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version)
|
||||
{
|
||||
static uint8_t pushBuf[30];
|
||||
int ret = 0;
|
||||
static uint8_t pushBuf[MSP_PORT_PUSH_BUFFER_SIZE];
|
||||
|
||||
mspPacket_t push = {
|
||||
.buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), },
|
||||
|
@ -508,6 +507,17 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
|
|||
.result = 0,
|
||||
};
|
||||
|
||||
sbufWriteData(&push.buf, data, datalen);
|
||||
|
||||
sbufSwitchToReader(&push.buf, pushBuf);
|
||||
|
||||
return mspSerialEncode(mspPort, &push, version);
|
||||
}
|
||||
|
||||
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t * const mspPort = &mspPorts[portIndex];
|
||||
if (!mspPort->port) {
|
||||
|
@ -519,11 +529,7 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
|
|||
continue;
|
||||
}
|
||||
|
||||
sbufWriteData(&push.buf, data, datalen);
|
||||
|
||||
sbufSwitchToReader(&push.buf, pushBuf);
|
||||
|
||||
ret = mspSerialEncode(mspPort, &push, MSP_V1);
|
||||
ret = mspSerialPushPort(cmd, data, datalen, mspPort, MSP_V1);
|
||||
}
|
||||
return ret; // return the number of bytes written
|
||||
}
|
||||
|
@ -551,3 +557,14 @@ uint32_t mspSerialTxBytesFree(void)
|
|||
|
||||
return ret;
|
||||
}
|
||||
|
||||
mspPort_t * mspSerialPortFind(const serialPort_t *serialPort)
|
||||
{
|
||||
for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t * mspPort = &mspPorts[portIndex];
|
||||
if (mspPort->port == serialPort) {
|
||||
return mspPort;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
|
|
@ -67,6 +67,7 @@ typedef enum {
|
|||
#else
|
||||
#define MSP_PORT_OUTBUF_SIZE 512
|
||||
#endif
|
||||
#define MSP_PORT_PUSH_BUFFER_SIZE 30 // Used by mspSerialPush* functions
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t size;
|
||||
|
@ -106,5 +107,7 @@ void mspSerialInit(void);
|
|||
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);
|
||||
void mspSerialAllocatePorts(void);
|
||||
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
|
||||
int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version);
|
||||
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen);
|
||||
uint32_t mspSerialTxBytesFree(void);
|
||||
mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue