mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
implement SITL in gazebosim with ArduCopterPlugin
need implement fake eeprom & fake IO need implement fake system function can compile, stuck in isEEPROMContentValid() EEPROM in memory work EEPROM as file should work fix some complie warning MSP over TCP work (add dyad.c) a little clean up fix FLASH_CONFIG_Size in ld script & implement some pwmout IO to simulator work!! need to check direction & scale!! can fly but Gyro buggy move dyad.c fix busy-loop (limit to max 20kHz) can simulatie in different speed now! (hard code) add option for IMU calculation add README.md move dyad.c and fix F3 overrun the flash size explanation SITL in README.md and reuse CFLAGS, ASFLAGS
This commit is contained in:
cs8425
parent
c2307a4bea
commit
120fa21693
23 changed files with 2668 additions and 3 deletions
268
src/main/drivers/serial_tcp.c
Normal file
268
src/main/drivers/serial_tcp.c
Normal file
|
|
@ -0,0 +1,268 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Authors:
|
||||
* Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
|
||||
* Hamasaki/Timecop - Initial baseflight code
|
||||
*/
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "serial_tcp.h"
|
||||
|
||||
#define BASE_PORT 5760
|
||||
|
||||
const struct serialPortVTable uartVTable[]; // Forward
|
||||
static uartPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
|
||||
static bool portInited[SERIAL_PORT_COUNT];
|
||||
static bool tcpStart = false;
|
||||
bool tcpIsStart(void) {
|
||||
return tcpStart;
|
||||
}
|
||||
static void onData(dyad_Event *e) {
|
||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
||||
tcpDataIn(s, (uint8_t*)e->data, e->size);
|
||||
}
|
||||
static void onClose(dyad_Event *e) {
|
||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
||||
s->clientCount--;
|
||||
s->conn = NULL;
|
||||
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
||||
if(s->clientCount == 0) {
|
||||
s->connected = false;
|
||||
}
|
||||
}
|
||||
static void onAccept(dyad_Event *e) {
|
||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
||||
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
|
||||
|
||||
s->connected = true;
|
||||
if(s->clientCount > 0) {
|
||||
dyad_close(e->remote);
|
||||
return;
|
||||
}
|
||||
s->clientCount++;
|
||||
fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
||||
s->conn = e->remote;
|
||||
dyad_setNoDelay(e->remote, 1);
|
||||
dyad_setTimeout(e->remote, 120);
|
||||
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
|
||||
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
|
||||
}
|
||||
static uartPort_t* tcpReconfigure(uartPort_t *s, int id)
|
||||
{
|
||||
if(portInited[id]) {
|
||||
fprintf(stderr, "port had initialed!!\n");
|
||||
return s;
|
||||
}
|
||||
|
||||
if (pthread_mutex_init(&s->txLock, NULL) != 0) {
|
||||
fprintf(stderr, "TX mutex init failed - %d\n", errno);
|
||||
// TODO: clean up & re-init
|
||||
return NULL;
|
||||
}
|
||||
if (pthread_mutex_init(&s->rxLock, NULL) != 0) {
|
||||
fprintf(stderr, "RX mutex init failed - %d\n", errno);
|
||||
// TODO: clean up & re-init
|
||||
return NULL;
|
||||
}
|
||||
|
||||
tcpStart = true;
|
||||
portInited[id] = true;
|
||||
|
||||
s->connected = false;
|
||||
s->clientCount = 0;
|
||||
s->id = id;
|
||||
s->conn = NULL;
|
||||
s->serv = dyad_newStream();
|
||||
dyad_setNoDelay(s->serv, 1);
|
||||
dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s);
|
||||
|
||||
if(dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) {
|
||||
fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id+1);
|
||||
} else {
|
||||
fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id+1);
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options)
|
||||
{
|
||||
uartPort_t *s = NULL;
|
||||
|
||||
#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
|
||||
uintptr_t id = ((uintptr_t)USARTx - 1);
|
||||
s = tcpReconfigure(&tcpSerialPorts[id], id);
|
||||
#else
|
||||
return (serialPort_t *)s;
|
||||
#endif
|
||||
|
||||
s->port.vTable = uartVTable;
|
||||
|
||||
// common serial initialisation code should move to serialPort::init()
|
||||
s->port.rxBufferHead = s->port.rxBufferTail = 0;
|
||||
s->port.txBufferHead = s->port.txBufferTail = 0;
|
||||
s->port.rxBufferSize = RX_BUFFER_SIZE;
|
||||
s->port.txBufferSize = TX_BUFFER_SIZE;
|
||||
s->port.rxBuffer = s->rxBuffer;
|
||||
s->port.txBuffer = s->txBuffer;
|
||||
|
||||
// callback works for IRQ-based RX ONLY
|
||||
s->port.rxCallback = rxCallback;
|
||||
s->port.mode = mode;
|
||||
s->port.baudRate = baudRate;
|
||||
s->port.options = options;
|
||||
|
||||
return (serialPort_t *)s;
|
||||
}
|
||||
|
||||
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t*)instance;
|
||||
uint32_t count;
|
||||
pthread_mutex_lock(&s->rxLock);
|
||||
if (s->port.rxBufferHead >= s->port.rxBufferTail) {
|
||||
count = s->port.rxBufferHead - s->port.rxBufferTail;
|
||||
} else {
|
||||
count = s->port.rxBufferSize + s->port.rxBufferHead - s->port.rxBufferTail;
|
||||
}
|
||||
pthread_mutex_unlock(&s->rxLock);
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t*)instance;
|
||||
uint32_t bytesUsed;
|
||||
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
if (s->port.txBufferHead >= s->port.txBufferTail) {
|
||||
bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
|
||||
} else {
|
||||
bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
|
||||
}
|
||||
bytesUsed = (s->port.txBufferSize - 1) - bytesUsed;
|
||||
pthread_mutex_unlock(&s->txLock);
|
||||
|
||||
return bytesUsed;
|
||||
}
|
||||
|
||||
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
bool isEmpty = s->port.txBufferTail == s->port.txBufferHead;
|
||||
pthread_mutex_unlock(&s->txLock);
|
||||
return isEmpty;
|
||||
}
|
||||
|
||||
uint8_t tcpRead(serialPort_t *instance)
|
||||
{
|
||||
uint8_t ch;
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
pthread_mutex_lock(&s->rxLock);
|
||||
|
||||
ch = s->port.rxBuffer[s->port.rxBufferTail];
|
||||
if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) {
|
||||
s->port.rxBufferTail = 0;
|
||||
} else {
|
||||
s->port.rxBufferTail++;
|
||||
}
|
||||
pthread_mutex_unlock(&s->rxLock);
|
||||
|
||||
return ch;
|
||||
}
|
||||
|
||||
void tcpWrite(serialPort_t *instance, uint8_t ch)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
|
||||
s->port.txBuffer[s->port.txBufferHead] = ch;
|
||||
if (s->port.txBufferHead + 1 >= s->port.txBufferSize) {
|
||||
s->port.txBufferHead = 0;
|
||||
} else {
|
||||
s->port.txBufferHead++;
|
||||
}
|
||||
pthread_mutex_unlock(&s->txLock);
|
||||
|
||||
tcpDataOut(s);
|
||||
}
|
||||
|
||||
void tcpDataOut(uartPort_t *instance)
|
||||
{
|
||||
uint32_t bytesUsed;
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
if(s->conn == NULL) return;
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
|
||||
if (s->port.txBufferHead < s->port.txBufferTail) {
|
||||
bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
|
||||
dyad_write(s->conn, (const void *)(s->port.txBuffer + s->port.txBufferTail), s->port.txBufferSize - s->port.txBufferTail);
|
||||
s->port.txBufferTail = 0;
|
||||
}
|
||||
bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
|
||||
dyad_write(s->conn, (const void *)(&(s->port.txBuffer[s->port.txBufferTail])), bytesUsed);
|
||||
s->port.txBufferTail = s->port.txBufferHead;
|
||||
|
||||
pthread_mutex_unlock(&s->txLock);
|
||||
}
|
||||
|
||||
void tcpDataIn(uartPort_t *instance, uint8_t* ch, int size)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
pthread_mutex_lock(&s->rxLock);
|
||||
|
||||
while(size--) {
|
||||
// printf("%c", *ch);
|
||||
s->port.rxBuffer[s->port.rxBufferHead] = *(ch++);
|
||||
if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) {
|
||||
s->port.rxBufferHead = 0;
|
||||
} else {
|
||||
s->port.rxBufferHead++;
|
||||
}
|
||||
}
|
||||
pthread_mutex_unlock(&s->rxLock);
|
||||
// printf("\n");
|
||||
}
|
||||
|
||||
const struct serialPortVTable uartVTable[] = {
|
||||
{
|
||||
.serialWrite = tcpWrite,
|
||||
.serialTotalRxWaiting = tcpTotalRxBytesWaiting,
|
||||
.serialTotalTxFree = tcpTotalTxBytesFree,
|
||||
.serialRead = tcpRead,
|
||||
.serialSetBaudRate = NULL,
|
||||
.isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
|
||||
.setMode = NULL,
|
||||
.writeBuf = NULL,
|
||||
.beginWrite = NULL,
|
||||
.endWrite = NULL,
|
||||
}
|
||||
};
|
||||
Loading…
Add table
Add a link
Reference in a new issue