Skip to content

Instantly share code, notes, and snippets.

@max-verem

max-verem/0001-Impelement-real-serial-support-for-SITL.patch

Created December 10, 2025 18:17
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save max-verem/e74a4752e82b29c3d3a9b8ae02a69e4a to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save max-verem/e74a4752e82b29c3d3a9b8ae02a69e4a to your computer and use it in GitHub Desktop.
Download ZIP
lab #8 patches [4.5.3-lab8]
Raw
0001-Impelement-real-serial-support-for-SITL.patch
From d53ba04c85c85c81dc8bdeec38bcfb8f41ebb471 Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 17:09:41 +0200
Subject: [PATCH 1/6] Impelement real serial support for SITL.
---
src/main/drivers/serial_sitl.c | 504 +++++++++++++++++++++++++++++++++
src/main/drivers/serial_sitl.h | 5 +
src/main/io/serial.c | 8 +
src/main/main.c | 11 +
src/main/target/SITL/target.mk | 1 +
5 files changed, 529 insertions(+)
create mode 100644 src/main/drivers/serial_sitl.c
create mode 100644 src/main/drivers/serial_sitl.h
diff --git a/src/main/drivers/serial_sitl.c b/src/main/drivers/serial_sitl.c
new file mode 100644
index 000000000..6a2a51869
--- /dev/null
+++ b/src/main/drivers/serial_sitl.c
@@ -0,0 +1,504 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <asm/termbits.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <pthread.h>
+#include <errno.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "io/serial.h"
+#include "serial_sitl.h"
+
+#include "serial_tcp.h"
+
+static pthread_mutex_t cb_mutex = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP;
+
+int serial_sitl_cb_lock(void)
+{
+ return pthread_mutex_lock(&cb_mutex);
+}
+
+int serial_sitl_cb_unlock(void)
+{
+ return pthread_mutex_unlock(&cb_mutex);
+}
+
+#define SERIAL_SITL_BUFFER_PAW 8
+#define SERIAL_SITL_BUFFER_SIZE (1 << SERIAL_SITL_BUFFER_PAW)
+#define SERIAL_SITL_BUFFER_MASK (SERIAL_SITL_BUFFER_SIZE - 1)
+
+//#define SERIAL_SITL_DEBUG1
+
+typedef struct
+{
+ int fd, id;
+ struct
+ {
+ uint8_t buffer[SERIAL_SITL_BUFFER_SIZE];
+ uint8_t linear[SERIAL_SITL_BUFFER_SIZE];
+ uint8_t queue[SERIAL_SITL_BUFFER_SIZE];
+ uint64_t head, tail;
+ int queue_len;
+ pthread_t th;
+ pthread_mutex_t lock;
+ pthread_cond_t cond;
+ } rx, tx;
+} serial_sitl_ctx_t;
+
+typedef struct
+{
+ serialPort_t port;
+ serial_sitl_ctx_t *ctx;
+
+} serial_sitl_port_t;
+
+static void copy_to_tx_buffer(serialPort_t *instance, const uint8_t* data, int count)
+{
+ int i;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+ for(i = 0; i < count; i++)
+ {
+ instance->txBuffer[instance->txBufferHead] = data[i];
+ ctx->tx.head++;
+ instance->txBufferHead = ctx->tx.head & SERIAL_SITL_BUFFER_MASK;
+ };
+}
+
+static serial_sitl_port_t ports[SERIAL_PORT_COUNT] = {0};
+static serial_sitl_ctx_t ctxs[SERIAL_PORT_COUNT] = {0};
+
+static void _serialWrite(serialPort_t *instance, uint8_t ch)
+{
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+#endif
+ pthread_mutex_lock(&ctx->tx.lock);
+ copy_to_tx_buffer(instance, &ch, 1);
+ pthread_cond_broadcast(&ctx->tx.cond);
+ pthread_mutex_unlock(&ctx->tx.lock);
+}
+
+static uint32_t _serialTotalRxWaiting(const serialPort_t *instance)
+{
+ uint32_t r;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+ pthread_mutex_lock(&ctx->rx.lock);
+ r = ctx->rx.head - ctx->rx.tail;
+ pthread_mutex_unlock(&ctx->rx.lock);
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s %d\n", ctx->id + 1, __func__, r);
+#endif
+ return r;
+}
+
+static uint32_t _serialTotalTxFree(const serialPort_t *instance)
+{
+ uint32_t r;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+ pthread_mutex_lock(&ctx->tx.lock);
+ r = SERIAL_SITL_BUFFER_SIZE - (ctx->tx.head - ctx->tx.tail);
+ pthread_mutex_unlock(&ctx->tx.lock);
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s %d\n", ctx->id + 1, __func__, r);
+#endif
+ return r;
+}
+
+static uint8_t _serialRead(serialPort_t *instance)
+{
+ uint8_t c = 0xAA;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+#endif
+
+ pthread_mutex_lock(&ctx->rx.lock);
+ c = instance->rxBuffer[instance->rxBufferTail];
+ if(ctx->rx.tail < ctx->rx.head)
+ {
+ ctx->rx.tail++;
+ instance->rxBufferTail = ctx->rx.tail & SERIAL_SITL_BUFFER_MASK;
+ };
+ pthread_mutex_unlock(&ctx->rx.lock);
+
+ return c;
+}
+
+static bool _isSerialTransmitBufferEmpty(const serialPort_t *instance)
+{
+ int r;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+#endif
+ pthread_mutex_lock(&ctx->tx.lock);
+ r = (ctx->tx.head == ctx->tx.tail);
+ pthread_mutex_unlock(&ctx->tx.lock);
+
+ return r;
+}
+
+
+static void _writeBuf(serialPort_t *instance, const void *data, int count)
+{
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s %d\n", ctx->id + 1, __func__, count);
+#endif
+ if(ctx->tx.queue_len < 0)
+ {
+ pthread_mutex_lock(&ctx->tx.lock);
+ copy_to_tx_buffer(instance, data, count);
+ pthread_cond_broadcast(&ctx->tx.cond);
+ pthread_mutex_unlock(&ctx->tx.lock);
+ }
+ else
+ {
+ memcpy(ctx->tx.queue + ctx->tx.queue_len, data, count);
+ ctx->tx.queue_len += count;
+ };
+}
+
+static void _beginWrite(serialPort_t *instance)
+{
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+#endif
+ if(ctx->tx.queue_len < 0)
+ ctx->tx.queue_len = 0;
+}
+
+static void _endWrite(serialPort_t *instance)
+{
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+#endif
+ if(ctx->tx.queue_len <= 0)
+ return;
+
+ pthread_mutex_lock(&ctx->tx.lock);
+ copy_to_tx_buffer(instance, ctx->tx.queue, ctx->tx.queue_len);
+ pthread_cond_broadcast(&ctx->tx.cond);
+ pthread_mutex_unlock(&ctx->tx.lock);
+
+ ctx->tx.queue_len = -1;
+}
+
+static const struct serialPortVTable serialSitlVTable =
+{
+ .serialWrite = _serialWrite,
+ .serialTotalRxWaiting = _serialTotalRxWaiting,
+ .serialTotalTxFree = _serialTotalTxFree,
+ .serialRead = _serialRead,
+ .serialSetBaudRate = NULL,
+ .isSerialTransmitBufferEmpty = _isSerialTransmitBufferEmpty,
+ .setMode = NULL,
+ .setCtrlLineStateCb = NULL,
+ .setBaudRateCb = NULL,
+ .writeBuf = _writeBuf,
+ .beginWrite = _beginWrite,
+ .endWrite = _endWrite,
+};
+
+#define READ_BUF 128
+
+#define tcflush(FD, FL) ioctl((FD), TCFLSH, (FL));
+
+static void* _rx_proc(void* p)
+{
+ int i, r;
+ serialPort_t *instance = (serialPort_t *)p;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+
+ tcflush(ctx->fd, TCIFLUSH);
+
+rx_loop:
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: ctx->rx.head=%lu, ctx->rx.tail=%lu\n", ctx->id + 1, ctx->rx.head, ctx->rx.tail);
+#endif
+ r = read(ctx->fd, ctx->rx.linear, READ_BUF);
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: read()=%d\n", ctx->id + 1, r);
+#endif
+
+ if(r < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: read failed: r=%d (%s)\n", ctx->id + 1, r, strerror(r));
+ return NULL;
+ };
+
+ if (instance->rxCallback)
+ {
+ pthread_mutex_lock(&cb_mutex);
+ for(i = 0; i < r; i++)
+ instance->rxCallback(ctx->rx.linear[i], instance->rxCallbackData);
+ pthread_mutex_unlock(&cb_mutex);
+ }
+ else
+ {
+ pthread_mutex_lock(&ctx->rx.lock);
+ for(i = 0; i < r; i++)
+ {
+ instance->rxBuffer[instance->rxBufferHead] = ctx->rx.linear[i];
+ ctx->rx.head++;
+ instance->rxBufferHead = ctx->rx.head & SERIAL_SITL_BUFFER_MASK;
+ };
+ pthread_cond_broadcast(&ctx->rx.cond);
+ pthread_mutex_unlock(&ctx->rx.lock);
+ };
+
+ goto rx_loop;
+
+ return NULL;
+}
+
+static void* _tx_proc(void* p)
+{
+ int r, s;
+
+ serialPort_t *instance = (serialPort_t *)p;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, __func__);
+
+ tcflush(ctx->fd, TCOFLUSH);
+
+tx_loop:
+
+ pthread_mutex_lock(&ctx->tx.lock);
+
+ while(ctx->tx.head == ctx->tx.tail)
+ pthread_cond_wait(&ctx->tx.cond, &ctx->tx.lock);
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: ctx->tx.head=%lu, ctx->tx.tail=%lu\n", ctx->id + 1, ctx->tx.head, ctx->tx.tail);
+#endif
+ /* unwrap buffer */
+ for(s = 0; s < SERIAL_SITL_BUFFER_SIZE && ctx->tx.head != ctx->tx.tail; s++)
+ {
+ ctx->tx.linear[s] = instance->txBuffer[instance->txBufferTail];
+ ctx->tx.tail++;
+ instance->txBufferTail = ctx->tx.tail & SERIAL_SITL_BUFFER_MASK;
+ };
+
+ pthread_mutex_unlock(&ctx->tx.lock);
+
+ r = write(ctx->fd, ctx->tx.linear, s);
+
+#ifdef SERIAL_SITL_DEBUG1
+ fprintf(stderr, "UART%d: %s r=%d s=%d\n", ctx->id + 1, __func__, r, s);
+#endif
+
+ if(r < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: write failed: r=%d (%s)\n", ctx->id + 1, r, strerror(r));
+ return NULL;
+ };
+
+ if(r != s)
+ {
+ fprintf(stderr, "UART%d: write(%d) != %d\n", ctx->id + 1, s, r);
+ return NULL;
+ };
+
+ goto tx_loop;
+
+ return NULL;
+}
+
+static void _init(serialPort_t *instance)
+{
+ int r, baudrate;
+ char *dev, *env_baudrate, tmp[64];
+ struct termios2 tio;
+ pthread_mutexattr_t mta;
+ serial_sitl_ctx_t* ctx = ((serial_sitl_port_t*)instance)->ctx;
+
+#if !defined BOTHER
+ fprintf(stderr, "UART%d: not supported\n", ctx->id + 1);
+ return;
+#endif
+
+ /* check if it TTY device */
+ snprintf(tmp, sizeof(tmp), "UART%d_TTY", ctx->id + 1);
+ dev = getenv(tmp);
+ if(!dev)
+ {
+ fprintf(stderr, "UART%d: env %s not defined\n", ctx->id + 1, tmp);
+ return;
+ };
+
+ fprintf(stderr, "UART%d: %s\n", ctx->id + 1, dev);
+
+ /* open serial */
+ ctx->fd = open(dev, O_RDWR | O_NOCTTY);
+ if(ctx->fd < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: failed to open %s: r=%d (%s)\n", ctx->id + 1, dev, r, strerror(r));
+ return;
+ };
+
+ /* check if baudrate specified */
+ snprintf(tmp, sizeof(tmp), "UART%d_BAUDRATE", ctx->id + 1);
+ env_baudrate = getenv(tmp);
+ if(!env_baudrate)
+ baudrate = instance->baudRate;
+ else
+ baudrate = atoi(env_baudrate);
+ fprintf(stderr, "UART%d: will try baudrate=%d\n", ctx->id + 1, baudrate);
+
+ /* get term settings */
+ r = ioctl(ctx->fd, TCGETS2, &tio);
+ if(r < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: ioctl(TCGETS2) failed: r=%d (%s)\n", ctx->id + 1, r, strerror(r));
+ close(ctx->fd); ctx->fd = -1;
+ return;
+ };
+
+ /* setup term */
+ tio.c_iflag = IGNPAR; /* ignore bytes with parity errors */
+ tio.c_oflag = 0; /* Raw output */
+ tio.c_lflag = 0; /* enable canonical input */
+ tio.c_cc[VTIME] = 1; /* inter-character timer unused */
+ tio.c_cc[VMIN] = 1; /* blocking read until 5 chars received */
+ tio.c_cflag =
+ CS8 |
+ CREAD |
+ CLOCAL;
+
+ /* Clear the current output baud rate and fill a new value */
+ tio.c_cflag &= ~CBAUD;
+ tio.c_cflag |= BOTHER;
+ tio.c_ospeed = baudrate;
+
+ /* Clear the current input baud rate and fill a new value */
+ tio.c_cflag &= ~(CBAUD << IBSHIFT);
+ tio.c_cflag |= BOTHER << IBSHIFT;
+ tio.c_ispeed = baudrate;
+
+ /* setup */
+ r = ioctl(ctx->fd, TCSETS2, &tio);
+ if(r < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: ioctl(TCSETS2) failed: r=%d (%s)\n", ctx->id + 1, r, strerror(r));
+ close(ctx->fd); ctx->fd = -1;
+ return;
+ };
+
+ /* get back */
+ r = ioctl(ctx->fd, TCGETS2, &tio);
+ if(r < 0)
+ {
+ r = errno;
+ fprintf(stderr, "UART%d: ioctl(TCGETS2) failed: r=%d (%s)\n", ctx->id + 1, r, strerror(r));
+ close(ctx->fd); ctx->fd = -1;
+ return;
+ };
+
+ /* compare baudrate setting */
+ if((int)tio.c_ospeed != baudrate || (int)tio.c_ispeed != baudrate)
+ fprintf(stderr, "UART%d: baudrate=%d, tio.c_ospeed=%d, tio.c_ispeed=%d\n",
+ ctx->id + 1, baudrate, tio.c_ospeed, tio.c_ispeed);
+
+ /* init mutex and conditional variables */
+ pthread_mutexattr_init(&mta);
+ pthread_mutexattr_settype(&mta, PTHREAD_MUTEX_RECURSIVE);
+ pthread_mutex_init(&ctx->tx.lock, &mta);
+ pthread_mutex_init(&ctx->rx.lock, &mta);
+ pthread_cond_init(&ctx->tx.cond, NULL);
+ pthread_cond_init(&ctx->rx.cond, NULL);
+
+ ctx->tx.queue_len = -1;
+ tcflush(ctx->fd, TCIOFLUSH);
+
+ /* create worker threads */
+ pthread_create(&ctx->tx.th, NULL, _tx_proc, instance);
+ pthread_create(&ctx->rx.th, NULL, _rx_proc, instance);
+}
+
+serialPort_t *serial_sitl_open(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
+{
+ char tmp[64];
+ serialPort_t *port;
+ serial_sitl_ctx_t* ctx;
+ serial_sitl_port_t* p = &ports[id];
+
+ fprintf(stderr, "%s(id=%d, baudRate=%d, mode=%d): entering\n", __func__, id, baudRate, mode);
+
+ /* check if env variable specified */
+ snprintf(tmp, sizeof(tmp), "UART%d_TTY", id + 1);
+ if(!getenv(tmp))
+ {
+ fprintf(stderr, "%s(id=%d, baudRate=%d, mode=%d): no [%s] defined - fallback to TCP driver\n",
+ __func__, id, baudRate, mode, tmp);
+ return serTcpOpen(id, rxCallback, rxCallbackData, baudRate, mode, options);
+ };
+
+ if(id < 0 || id >= SERIAL_PORT_COUNT)
+ return NULL;
+
+ ctx = p->ctx;
+ port = &p->port;
+
+ if(ctx)
+ {
+ fprintf(stderr, "%s(id=%d, baudRate=%d, mode=%d): already initialized\n", __func__, id, baudRate, mode);
+ return port;
+ };
+
+ ctx = p->ctx = &ctxs[id];
+
+ port->vTable = &serialSitlVTable;
+
+ // common serial initialisation code should move to serialPort::init()
+ port->rxBufferHead = port->rxBufferTail = 0;
+ port->txBufferHead = port->txBufferTail = 0;
+ port->rxBufferSize = SERIAL_SITL_BUFFER_SIZE;
+ port->txBufferSize = SERIAL_SITL_BUFFER_SIZE;
+ port->rxBuffer = ctx->rx.buffer;
+ port->txBuffer = ctx->tx.buffer;
+
+ // callback works for IRQ-based RX ONLY
+ port->rxCallback = rxCallback;
+ port->rxCallbackData = rxCallbackData;
+ port->mode = mode;
+ port->baudRate = baudRate;
+ port->options = options;
+
+ ctx->id = id;
+ _init(port);
+
+ return port;
+}
diff --git a/src/main/drivers/serial_sitl.h b/src/main/drivers/serial_sitl.h
new file mode 100644
index 000000000..aa5620f98
--- /dev/null
+++ b/src/main/drivers/serial_sitl.h
@@ -0,0 +1,5 @@
+#pragma once
+
+serialPort_t *serial_sitl_open(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options);
+int serial_sitl_cb_lock(void);
+int serial_sitl_cb_unlock(void);
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index ddfbd8741..6c7545196 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -40,8 +40,12 @@
#endif
#if defined(SIMULATOR_BUILD)
+#if defined(USE_SERIAL_SITL)
+#include "drivers/serial_sitl.h"
+#else
#include "drivers/serial_tcp.h"
#endif
+#endif
#include "drivers/light_led.h"
@@ -486,8 +490,12 @@ serialPort_t *openSerialPort(
case SERIAL_PORT_LPUART1:
#endif
#if defined(SIMULATOR_BUILD)
+#if defined(USE_SERIAL_SITL)
+ serialPort = serial_sitl_open(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, rxCallbackData, baudRate, mode, options);
+#else
// emulate serial ports over TCP
serialPort = serTcpOpen(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, rxCallbackData, baudRate, mode, options);
+#endif
#else
serialPort = uartOpen(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, rxCallbackData, baudRate, mode, options);
#endif
diff --git a/src/main/main.c b/src/main/main.c
index a2a97a4e9..dd3f2bcb5 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -44,10 +44,21 @@ int main(int argc, char * argv[])
return 0;
}
+#if defined(SIMULATOR_BUILD) && defined(USE_SERIAL_SITL)
+int serial_sitl_cb_lock(void);
+int serial_sitl_cb_unlock(void);
+#endif
+
void FAST_CODE run(void)
{
while (true) {
+#if defined(SIMULATOR_BUILD) && defined(USE_SERIAL_SITL)
+ serial_sitl_cb_lock();
+#endif
scheduler();
+#if defined(SIMULATOR_BUILD) && defined(USE_SERIAL_SITL)
+ serial_sitl_cb_unlock();
+#endif
#ifdef SIMULATOR_BUILD
delayMicroseconds_real(50); // max rate 20kHz
#endif
diff --git a/src/main/target/SITL/target.mk b/src/main/target/SITL/target.mk
index 7429467dc..243eaf52b 100644
--- a/src/main/target/SITL/target.mk
+++ b/src/main/target/SITL/target.mk
@@ -6,4 +6,5 @@ TARGET_SRC = \
drivers/accgyro/accgyro_virtual.c \
drivers/barometer/barometer_virtual.c \
drivers/compass/compass_virtual.c \
+ drivers/serial_sitl.c \
drivers/serial_tcp.c
--
2.47.3
Raw
0002-Prevent-SITL-slowdown.patch
From 43708ac5f34d07e21120a3d5b69cf394b3feedfe Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 17:25:44 +0200
Subject: [PATCH 2/6] Prevent SITL slowdown
---
src/main/target/SITL/sitl.c | 1 -
1 file changed, 1 deletion(-)
diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c
index 10297e674..269b89be7 100644
--- a/src/main/target/SITL/sitl.c
+++ b/src/main/target/SITL/sitl.c
@@ -188,7 +188,6 @@ void updateState(const fdm_packet* pkt)
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts);
- simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
}
// printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
--
2.47.3
Raw
0003-Implement-microsISR-as-it-required-by-serial-drivers.patch
From 4b7164b045d35b058276c0af48acfe6f5dac7f4e Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 17:27:05 +0200
Subject: [PATCH 3/6] Implement microsISR() as it required by serial drivers
that works with DMA callback
---
src/main/target/SITL/sitl.c | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c
index 269b89be7..d1f9c0f34 100644
--- a/src/main/target/SITL/sitl.c
+++ b/src/main/target/SITL/sitl.c
@@ -757,3 +757,8 @@ void IOLo(IO_t io)
{
UNUSED(io);
}
+
+uint32_t microsISR(void)
+{
+ return micros();
+}
--
2.47.3
Raw
0004-No-delay-on-main-loop-maximal-rt-performance.patch
From 7328aed43283f7fe3003a64d93a604cb4a862465 Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 17:28:40 +0200
Subject: [PATCH 4/6] No delay on main loop - maximal rt performance
---
src/main/target/SITL/sitl.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c
index d1f9c0f34..9cb51475a 100644
--- a/src/main/target/SITL/sitl.c
+++ b/src/main/target/SITL/sitl.c
@@ -465,7 +465,7 @@ void delayMicroseconds(uint32_t us)
void delayMicroseconds_real(uint32_t us)
{
- microsleep(us);
+ UNUSED(us);
}
void delay(uint32_t ms)
--
2.47.3
Raw
0005-Drop-ping-pong-scheme-for-PWM-FDM-packets-send-recei.patch
From 23c4bf2f0124c2bd7c6c9cd5b851f587f38a25a1 Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 17:32:58 +0200
Subject: [PATCH 5/6] Drop ping-pong scheme for PWM-FDM packets send/receive.
Use delayMicroseconds_real() as body for 'idle' function to perform IMU
updates
---
src/main/target/SITL/sitl.c | 36 ++++++++++++++++++++----------------
1 file changed, 20 insertions(+), 16 deletions(-)
diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c
index 9cb51475a..3c6f3b3a9 100644
--- a/src/main/target/SITL/sitl.c
+++ b/src/main/target/SITL/sitl.c
@@ -60,7 +60,9 @@
uint32_t SystemCoreClock;
-static fdm_packet fdmPkt;
+static fdm_packet fdmPkt_last;
+static int fdmPkt_cnt = 0;
+static pthread_mutex_t fdmPkt_lock = PTHREAD_MUTEX_INITIALIZER;
static rc_packet rcPkt;
static servo_packet pwmPkt;
static servo_packet_raw pwmRawPkt;
@@ -73,7 +75,6 @@ static double simRate = 1.0;
static pthread_t tcpWorker, udpWorker, udpWorkerRC;
static bool workerRunning = true;
static udpLink_t stateLink, pwmLink, pwmRawLink, rcLink;
-static pthread_mutex_t updateLock;
static pthread_mutex_t mainLoopLock;
static char simulator_ip[32] = "127.0.0.1";
@@ -197,8 +198,6 @@ void updateState(const fdm_packet* pkt)
last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec;
- pthread_mutex_unlock(&updateLock); // can send PWM output now
-
#if defined(SIMULATOR_GYROPID_SYNC)
pthread_mutex_unlock(&mainLoopLock); // can run main loop
#endif
@@ -208,6 +207,7 @@ static void* udpThread(void* data)
{
UNUSED(data);
int n = 0;
+ fdm_packet fdmPkt;
while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
@@ -216,7 +216,10 @@ static void* udpThread(void* data)
printf("[SITL] new fdm %d t:%f from %s:%d\n", n, fdmPkt.timestamp, inet_ntoa(stateLink.recv.sin_addr), stateLink.recv.sin_port);
fdm_received = true;
}
- updateState(&fdmPkt);
+ pthread_mutex_lock(&fdmPkt_lock);
+ fdmPkt_last = fdmPkt;
+ fdmPkt_cnt++;
+ pthread_mutex_unlock(&fdmPkt_lock);
}
}
@@ -290,11 +293,6 @@ void systemInit(void)
SystemCoreClock = 500 * 1e6; // virtual 500MHz
- if (pthread_mutex_init(&updateLock, NULL) != 0) {
- printf("Create updateLock error!\n");
- exit(1);
- }
-
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
printf("Create mainLoopLock error!\n");
exit(1);
@@ -466,6 +464,18 @@ void delayMicroseconds(uint32_t us)
void delayMicroseconds_real(uint32_t us)
{
UNUSED(us);
+ int r = 0;
+ fdm_packet fdmPkt;
+ UNUSED(us); // no delay in main loop
+
+ pthread_mutex_lock(&fdmPkt_lock);
+ r = fdmPkt_cnt;
+ fdmPkt_cnt = 0;
+ fdmPkt = fdmPkt_last;
+ pthread_mutex_unlock(&fdmPkt_lock);
+
+ if(r)
+ updateState(&fdmPkt);
}
void delay(uint32_t ms)
@@ -550,8 +560,6 @@ static bool pwmEnableMotors(void)
static void pwmWriteMotor(uint8_t index, float value)
{
- if (pthread_mutex_trylock(&updateLock) != 0) return;
-
if (index < MAX_SUPPORTED_MOTORS) {
motorsPwm[index] = value - idlePulse;
}
@@ -559,8 +567,6 @@ static void pwmWriteMotor(uint8_t index, float value)
if (index < pwmRawPkt.motorCount) {
pwmRawPkt.pwm_output_raw[index] = value;
}
-
- pthread_mutex_unlock(&updateLock); // can send PWM output now
}
static void pwmWriteMotorInt(uint8_t index, uint16_t value)
@@ -593,8 +599,6 @@ static void pwmCompleteMotorUpdate(void)
pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
- // get one "fdm_packet" can only send one "servo_packet"!!
- if (pthread_mutex_trylock(&updateLock) != 0) return;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
udpSend(&pwmRawLink, &pwmRawPkt, sizeof(servo_packet_raw));
--
2.47.3
Raw
0006-Allow-CRSF-receiving.patch
From 9ee26416886d3837f9d7a0a17385197a803c40dc Mon Sep 17 00:00:00 2001
From: Maksym Veremeyenko <[email protected]>
Date: Wed, 10 Dec 2025 20:12:53 +0200
Subject: [PATCH 6/6] Allow CRSF receiving
---
src/main/target/SITL/target.h | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 6032570c4..ca496a87a 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -111,8 +111,8 @@
#undef USE_OSD
#undef USE_RX_PPM
#undef USE_RX_PWM
-#undef USE_SERIALRX
-#undef USE_SERIALRX_CRSF
+//#undef USE_SERIALRX
+//#undef USE_SERIALRX_CRSF
#undef USE_SERIALRX_GHST
#undef USE_SERIALRX_IBUS
#undef USE_SERIALRX_SBUS
@@ -127,7 +127,7 @@
#undef USE_TELEMETRY_MAVLINK
#undef USE_RESOURCE_MGMT
#undef USE_CMS
-#undef USE_TELEMETRY_CRSF
+//#undef USE_TELEMETRY_CRSF
#undef USE_TELEMETRY_GHST
#undef USE_TELEMETRY_IBUS
#undef USE_TELEMETRY_JETIEXBUS
--
2.47.3
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment