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/system.c \
drivers/rcc.c \ drivers/rcc.c \
drivers/serial_uart.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 # check if target.mk supplied
@ -1138,7 +1143,7 @@ CC_SPEED_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
CC_SIZE_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SIZE) CC_SIZE_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SIZE)
else #DEBUG else #DEBUG
OPTIMISE_DEFAULT := -O0 OPTIMISE_DEFAULT := -Og
CC_DEBUG_OPTIMISATION := $(OPTIMISE_DEFAULT) CC_DEBUG_OPTIMISATION := $(OPTIMISE_DEFAULT)

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

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

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

@ -20,19 +20,14 @@
#include <netinet/in.h> #include <netinet/in.h>
#include <pthread.h> #include <pthread.h>
#include "dyad.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 RX_BUFFER_SIZE 1400
#define TX_BUFFER_SIZE 1400 #define TX_BUFFER_SIZE 1400
typedef struct { typedef struct {
serialPort_t port; serialPort_t port;
volatile uint8_t rxBuffer[RX_BUFFER_SIZE]; uint8_t rxBuffer[RX_BUFFER_SIZE];
volatile uint8_t txBuffer[TX_BUFFER_SIZE]; uint8_t txBuffer[TX_BUFFER_SIZE];
dyad_Stream *serv; dyad_Stream *serv;
dyad_Stream *conn; dyad_Stream *conn;
@ -43,16 +38,11 @@ typedef struct {
uint8_t id; uint8_t id;
} tcpPort_t; } 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 // tcpPort API
void tcpWrite(serialPort_t *instance, uint8_t ch);
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size); 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); void tcpDataOut(tcpPort_t *instance);
bool isTcpTransmitBufferEmpty(const serialPort_t *s);
bool tcpIsStart(void); bool tcpIsStart(void);
bool* tcpGetUsed(void); bool* tcpGetUsed(void);

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

@ -44,6 +44,10 @@
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#endif #endif
#ifdef SITL
#include "drivers/serial_tcp.h"
#endif
#include "drivers/light_led.h" #include "drivers/light_led.h"
#if defined(USE_VCP) #if defined(USE_VCP)
@ -379,7 +383,12 @@ serialPort_t *openSerialPort(
#ifdef USE_UART8 #ifdef USE_UART8
case SERIAL_PORT_USART8: case SERIAL_PORT_USART8:
#endif #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); serialPort = uartOpen(SERIAL_PORT_IDENTIFIER_TO_UARTDEV(identifier), rxCallback, baudRate, mode, options);
#endif
break; break;
#endif #endif

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

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

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

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

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

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

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

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