rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3115 from ledvinap/fix-SITL 

Fix SITL
This commit is contained in:
Michael Keller 2017-05-23 23:00:54 +12:00 committed by GitHub
parent 3170a88d1d 9c40903713
commit 468217c7e8
8 changed files with 63 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Makefile
View File

@ -1008,6 +1008,11 @@ SITLEXCLUDES = \
drivers/system.c \
drivers/rcc.c \
drivers/serial_uart.c \
drivers/rx_xn297.c \
drivers/display_ug2864hsweg01.c \
telemetry/crsf.c \
telemetry/srxl.c \
io/displayport_oled.c
# check if target.mk supplied
@ -1138,7 +1143,7 @@ CC_SPEED_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
CC_SIZE_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SIZE)
else #DEBUG
OPTIMISE_DEFAULT := -O0
OPTIMISE_DEFAULT := -Og
CC_DEBUG_OPTIMISATION := $(OPTIMISE_DEFAULT)

44
src/main/drivers/serial_tcp.c
View File

@ -37,9 +37,9 @@
#define BASE_PORT 5760
const struct serialPortVTable uartVTable[]; // Forward
static const struct serialPortVTable tcpVTable; // Forward
static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
static bool portInited[SERIAL_PORT_COUNT];
static bool tcpPortInitialized[SERIAL_PORT_COUNT];
static bool tcpStart = false;
bool tcpIsStart(void) {
return tcpStart;
@ -76,8 +76,8 @@ static void onAccept(dyad_Event *e) {
}
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
{
if(portInited[id]) {
fprintf(stderr, "port had initialed!!\n");
if(tcpPortInitialized[id]) {
fprintf(stderr, "port is already initialized!\n");
return s;
}
@ -93,7 +93,7 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
}
tcpStart = true;
portInited[id] = true;
tcpPortInitialized[id] = true;
s->connected = false;
s->clientCount = 0;
@ -104,25 +104,26 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
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);
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);
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)
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options)
{
tcpPort_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);
if(id >= 0 && id < SERIAL_PORT_COUNT) {
s = tcpReconfigure(&tcpSerialPorts[id], id);
#else
return (serialPort_t *)s;
}
#endif
if(!s)
return NULL;
s->port.vTable = uartVTable;
s->port.vTable = &tcpVTable;
// common serial initialisation code should move to serialPort::init()
s->port.rxBufferHead = s->port.rxBufferTail = 0;
@ -167,10 +168,10 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
} else {
bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
}
bytesUsed = (s->port.txBufferSize - 1) - bytesUsed;
uint32_t bytesFree = (s->port.txBufferSize - 1) - bytesUsed;
pthread_mutex_unlock(&s->txLock);
return bytesUsed;
return bytesFree;
}
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
@ -217,18 +218,19 @@ void tcpWrite(serialPort_t *instance, uint8_t ch)
void tcpDataOut(tcpPort_t *instance)
{
uint32_t bytesUsed;
tcpPort_t *s = (tcpPort_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);
// send data till end of buffer
int chunk = s->port.txBufferSize - s->port.txBufferTail;
dyad_write(s->conn, (const void *)&s->port.txBuffer[s->port.txBufferTail], chunk);
s->port.txBufferTail = 0;
}
bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
dyad_write(s->conn, (const void *)(&(s->port.txBuffer[s->port.txBufferTail])), bytesUsed);
int chunk = s->port.txBufferHead - s->port.txBufferTail;
if(chunk)
dyad_write(s->conn, (const void*)&s->port.txBuffer[s->port.txBufferTail], chunk);
s->port.txBufferTail = s->port.txBufferHead;
pthread_mutex_unlock(&s->txLock);
@ -252,8 +254,7 @@ void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
// printf("\n");
}
const struct serialPortVTable uartVTable[] = {
{
static const struct serialPortVTable tcpVTable = {
.serialWrite = tcpWrite,
.serialTotalRxWaiting = tcpTotalRxBytesWaiting,
.serialTotalTxFree = tcpTotalTxBytesFree,
@ -264,5 +265,4 @@ const struct serialPortVTable uartVTable[] = {
.writeBuf = NULL,
.beginWrite = NULL,
.endWrite = NULL,
}
};

16
src/main/drivers/serial_tcp.h
View File

@ -20,19 +20,14 @@
#include <netinet/in.h>
#include <pthread.h>
#include "dyad.h"
// Since serial ports can be used for any function these buffer sizes should be equal
// The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet.
// Size must be a power of two due to various optimizations which use 'and' instead of 'mod'
// Various serial routines return the buffer occupied size as uint8_t which would need to be extended in order to
// increase size further.
#define RX_BUFFER_SIZE 1400
#define TX_BUFFER_SIZE 1400
typedef struct {
serialPort_t port;
volatile uint8_t rxBuffer[RX_BUFFER_SIZE];
volatile uint8_t txBuffer[TX_BUFFER_SIZE];
uint8_t rxBuffer[RX_BUFFER_SIZE];
uint8_t txBuffer[TX_BUFFER_SIZE];
dyad_Stream *serv;
dyad_Stream *conn;
@ -43,16 +38,11 @@ typedef struct {
uint8_t id;
} tcpPort_t;
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options);
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options);
// tcpPort API
void tcpWrite(serialPort_t *instance, uint8_t ch);
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size);
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance);
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance);
uint8_t tcpRead(serialPort_t *instance);
void tcpDataOut(tcpPort_t *instance);
bool isTcpTransmitBufferEmpty(const serialPort_t *s);
bool tcpIsStart(void);
bool* tcpGetUsed(void);

9
src/main/io/serial.c
View File

@ -44,6 +44,10 @@
#include "drivers/serial_uart.h"
#endif
#ifdef SITL
#include "drivers/serial_tcp.h"
#endif
#include "drivers/light_led.h"
#if defined(USE_VCP)
@ -379,7 +383,12 @@ serialPort_t *openSerialPort(
#ifdef USE_UART8
case SERIAL_PORT_USART8:
#endif
#ifdef SITL
// SITL emulates serial ports over TCP
serialPort = serTcpOpen(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, baudRate, mode, options);
#else
serialPort = uartOpen(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, baudRate, mode, options);
#endif
break;
#endif

3
src/main/target/SITL/parameter_group.ld
View File

@ -25,8 +25,7 @@ INSERT AFTER .text;
__FLASH_CONFIG_Size = 0x2000; /* 8kB */
SECTIONS {
/* .FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) : SUBALIGN(4)*/
.FLASH_CONFIG :
.FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) :
{
PROVIDE_HIDDEN (__config_start = . );
. = . + __FLASH_CONFIG_Size;

43
src/main/target/SITL/target.c
View File

@ -436,63 +436,58 @@ char _Min_Stack_Size;
// fake EEPROM
extern uint8_t __config_start;
extern uint8_t __config_end;
extern uint32_t __FLASH_CONFIG_Size;
static FILE *eepromFd = NULL;
const char *EEPROM_FILE = EEPROM_FILENAME;
void FLASH_Unlock(void) {
uint8_t * const eeprom = &__config_start;
if(eepromFd != NULL) {
if (eepromFd != NULL) {
printf("[FLASH_Unlock] eepromFd != NULL\n");
return;
}
// open or create
eepromFd = fopen(EEPROM_FILE,"r+");
if(eepromFd != NULL) {
long lSize;
int c;
eepromFd = fopen(EEPROM_FILENAME,"r+");
if (eepromFd != NULL) {
// obtain file size:
fseek(eepromFd , 0 , SEEK_END);
lSize = ftell(eepromFd);
long lSize = ftell(eepromFd);
rewind(eepromFd);
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
for(unsigned int i=0; i<(uintptr_t)(&__config_end - &__config_start); i++){
c = fgetc(eepromFd);
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
int c = fgetc(eepromFd);
if(c == EOF) break;
eeprom[i] = (uint8_t)c;
}
}else{
eepromFd = fopen(EEPROM_FILE, "w+");
fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd);
} else {
eepromFd = fopen(EEPROM_FILENAME, "w+");
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
}
}
void FLASH_Lock(void) {
// flush & close
if(eepromFd != NULL) {
if (eepromFd != NULL) {
const uint8_t *eeprom = &__config_start;
fseek(eepromFd, 0, SEEK_SET);
fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd);
fflush(eepromFd);
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
fclose(eepromFd);
eepromFd = NULL;
}
}
FLASH_Status FLASH_ErasePage(uint32_t Page_Address) {
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
UNUSED(Page_Address);
// printf("[FLASH_ErasePage]%x\n", Page_Address);
return FLASH_COMPLETE;
}
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data) {
if((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
*((uint32_t*)(uintptr_t)addr) = Data;
// printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
}else{
printf("[FLASH_ProgramWord]Out of Range!! 0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
*((uint32_t*)addr) = Data;
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
} else {
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
}
return FLASH_COMPLETE;
}

4
src/main/target/SITL/target.h
View File

@ -233,8 +233,8 @@ typedef struct {
void FLASH_Unlock(void);
void FLASH_Lock(void);
FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data);
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address);
FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data);
uint64_t nanos64_real();
uint64_t micros64_real();

1
src/main/target/SITL/udplink.c
View File

@ -8,6 +8,7 @@
#include <string.h>
#include <fcntl.h>
#include <sys/socket.h>
#include "udplink.h"