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
69
Makefile
69
Makefile
|
|
@ -197,7 +197,7 @@ ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
|
||||||
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
|
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),)
|
ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS) $(SITL_TARGETS)),)
|
||||||
$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411 or F7x5. Have you prepared a valid target.mk?)
|
$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411 or F7x5. Have you prepared a valid target.mk?)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
|
@ -205,7 +205,7 @@ endif
|
||||||
256K_TARGETS = $(F3_TARGETS)
|
256K_TARGETS = $(F3_TARGETS)
|
||||||
512K_TARGETS = $(F411_TARGETS) $(F446_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS)
|
512K_TARGETS = $(F411_TARGETS) $(F446_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS)
|
||||||
1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_TARGETS)
|
1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_TARGETS)
|
||||||
2048K_TARGETS = $(F7X5XI_TARGETS)
|
2048K_TARGETS = $(F7X5XI_TARGETS) $(SITL_TARGETS)
|
||||||
|
|
||||||
# Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
|
# Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
|
||||||
ifeq ($(FLASH_SIZE),)
|
ifeq ($(FLASH_SIZE),)
|
||||||
|
|
@ -534,6 +534,26 @@ TARGET_FLAGS = -D$(TARGET)
|
||||||
|
|
||||||
# End F7 targets
|
# End F7 targets
|
||||||
#
|
#
|
||||||
|
# Start SITL targets
|
||||||
|
else ifeq ($(TARGET),$(filter $(TARGET), $(SITL_TARGETS)))
|
||||||
|
|
||||||
|
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||||
|
$(ROOT)/lib/main/dyad
|
||||||
|
|
||||||
|
SITL_SRC := $(ROOT)/lib/main/dyad/dyad.c
|
||||||
|
|
||||||
|
#Flags
|
||||||
|
ARCH_FLAGS =
|
||||||
|
DEVICE_FLAGS =
|
||||||
|
LD_SCRIPT = src/main/target/SITL/parameter_group.ld
|
||||||
|
STARTUP_SRC =
|
||||||
|
|
||||||
|
TARGET_FLAGS = -D$(TARGET)
|
||||||
|
|
||||||
|
ARM_SDK_PREFIX =
|
||||||
|
|
||||||
|
# End SITL targets
|
||||||
|
#
|
||||||
# Start F1 targets
|
# Start F1 targets
|
||||||
else
|
else
|
||||||
|
|
||||||
|
|
@ -939,6 +959,19 @@ F7EXCLUDES = drivers/bus_spi.c \
|
||||||
drivers/timer.c \
|
drivers/timer.c \
|
||||||
drivers/serial_uart.c
|
drivers/serial_uart.c
|
||||||
|
|
||||||
|
SITLEXCLUDES = \
|
||||||
|
drivers/adc.c \
|
||||||
|
drivers/bus_spi.c \
|
||||||
|
drivers/bus_i2c.c \
|
||||||
|
drivers/dma.c \
|
||||||
|
drivers/pwm_output.c \
|
||||||
|
drivers/timer.c \
|
||||||
|
drivers/light_led.c \
|
||||||
|
drivers/system.c \
|
||||||
|
drivers/rcc.c \
|
||||||
|
drivers/serial_uart.c \
|
||||||
|
|
||||||
|
|
||||||
# check if target.mk supplied
|
# check if target.mk supplied
|
||||||
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
|
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
|
||||||
SRC := $(STARTUP_SRC) $(STM32F4xx_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
SRC := $(STARTUP_SRC) $(STM32F4xx_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
||||||
|
|
@ -948,6 +981,8 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
|
||||||
SRC := $(STARTUP_SRC) $(STM32F30x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
SRC := $(STARTUP_SRC) $(STM32F30x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
||||||
else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
|
else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
|
||||||
SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
|
||||||
|
else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
|
||||||
|
SRC := $(TARGET_SRC) $(SITL_SRC) $(VARIANT_SRC)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),)
|
ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),)
|
||||||
|
|
@ -982,6 +1017,11 @@ ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
|
||||||
SRC := $(filter-out ${F7EXCLUDES}, $(SRC))
|
SRC := $(filter-out ${F7EXCLUDES}, $(SRC))
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
#SITL excludes
|
||||||
|
ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
|
||||||
|
SRC := $(filter-out ${SITLEXCLUDES}, $(SRC))
|
||||||
|
endif
|
||||||
|
|
||||||
ifneq ($(filter SDCARD,$(FEATURES)),)
|
ifneq ($(filter SDCARD,$(FEATURES)),)
|
||||||
SRC += \
|
SRC += \
|
||||||
drivers/sdcard.c \
|
drivers/sdcard.c \
|
||||||
|
|
@ -1037,6 +1077,13 @@ OPTIMISE_SIZE := -Os
|
||||||
|
|
||||||
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
|
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
|
||||||
|
|
||||||
|
else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
|
||||||
|
OPTIMISE_DEFAULT := -Ofast
|
||||||
|
OPTIMISE_SPEED := -Ofast
|
||||||
|
OPTIMISE_SIZE := -Os
|
||||||
|
|
||||||
|
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
|
||||||
|
|
||||||
else
|
else
|
||||||
OPTIMISE_DEFAULT := -Ofast
|
OPTIMISE_DEFAULT := -Ofast
|
||||||
|
|
||||||
|
|
@ -1097,6 +1144,24 @@ LDFLAGS = -lm \
|
||||||
-Wl,--no-wchar-size-warning \
|
-Wl,--no-wchar-size-warning \
|
||||||
-T$(LD_SCRIPT)
|
-T$(LD_SCRIPT)
|
||||||
|
|
||||||
|
#SITL compile
|
||||||
|
ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
|
||||||
|
LDFLAGS = \
|
||||||
|
-lm \
|
||||||
|
-lpthread \
|
||||||
|
-lc \
|
||||||
|
-lrt \
|
||||||
|
$(ARCH_FLAGS) \
|
||||||
|
$(LTO_FLAGS) \
|
||||||
|
$(DEBUG_FLAGS) \
|
||||||
|
-static \
|
||||||
|
-static-libgcc \
|
||||||
|
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
|
||||||
|
-Wl,-L$(LINKER_DIR) \
|
||||||
|
-Wl,--cref \
|
||||||
|
-T$(LD_SCRIPT)
|
||||||
|
endif
|
||||||
|
|
||||||
###############################################################################
|
###############################################################################
|
||||||
# No user-serviceable parts below
|
# No user-serviceable parts below
|
||||||
###############################################################################
|
###############################################################################
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
Copyright (c) 2016 rxi
|
||||||
|
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||||||
|
this software and associated documentation files (the "Software"), to deal in
|
||||||
|
the Software without restriction, including without limitation the rights to
|
||||||
|
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
|
||||||
|
of the Software, and to permit persons to whom the Software is furnished to do
|
||||||
|
so, subject to the following conditions:
|
||||||
|
|
||||||
|
The above copyright notice and this permission notice shall be included in all
|
||||||
|
copies or substantial portions of the Software.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||||
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||||
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||||
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||||
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
|
SOFTWARE.
|
||||||
|
|
@ -0,0 +1,85 @@
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## Overview
|
||||||
|
Dyad.c is an asynchronous networking library which aims to be lightweight,
|
||||||
|
portable and easy to use. It can be used both to create small standalone
|
||||||
|
servers and to provide network support to existing projects.
|
||||||
|
|
||||||
|
## Getting started
|
||||||
|
The [dyad.c](src/dyad.c?raw=1) and [dyad.h](src/dyad.h?raw=1) files can be
|
||||||
|
dropped into an existing project; if you're using Windows you will also have to
|
||||||
|
link to `ws2_32`.
|
||||||
|
|
||||||
|
An overview of the API can be found at [doc/api.md](doc/api.md).
|
||||||
|
|
||||||
|
Usage examples can be found at [example/](example/).
|
||||||
|
|
||||||
|
## Server example
|
||||||
|
A simple server which listens on port 8000 and echoes whatever is sent to it:
|
||||||
|
```c
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include "dyad.h"
|
||||||
|
|
||||||
|
static void onData(dyad_Event *e) {
|
||||||
|
dyad_write(e->stream, e->data, e->size);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void onAccept(dyad_Event *e) {
|
||||||
|
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, NULL);
|
||||||
|
dyad_writef(e->remote, "Echo server\r\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(void) {
|
||||||
|
dyad_init();
|
||||||
|
|
||||||
|
dyad_Stream *serv = dyad_newStream();
|
||||||
|
dyad_addListener(serv, DYAD_EVENT_ACCEPT, onAccept, NULL);
|
||||||
|
dyad_listen(serv, 8000);
|
||||||
|
|
||||||
|
while (dyad_getStreamCount() > 0) {
|
||||||
|
dyad_update();
|
||||||
|
}
|
||||||
|
|
||||||
|
dyad_shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## Client example
|
||||||
|
A simple example program which connects to a
|
||||||
|
[daytime](http://en.wikipedia.org/wiki/Daytime_Protocol) server and prints the
|
||||||
|
response:
|
||||||
|
```c
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "dyad.h"
|
||||||
|
|
||||||
|
static void onConnect(dyad_Event *e) {
|
||||||
|
printf("connected: %s\n", e->msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void onData(dyad_Event *e) {
|
||||||
|
printf("%s", e->data);
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(void) {
|
||||||
|
dyad_init();
|
||||||
|
|
||||||
|
dyad_Stream *s = dyad_newStream();
|
||||||
|
dyad_addListener(s, DYAD_EVENT_CONNECT, onConnect, NULL);
|
||||||
|
dyad_addListener(s, DYAD_EVENT_DATA, onData, NULL);
|
||||||
|
dyad_connect(s, "time-nw.nist.gov", 13);
|
||||||
|
|
||||||
|
while (dyad_getStreamCount() > 0) {
|
||||||
|
dyad_update();
|
||||||
|
}
|
||||||
|
|
||||||
|
dyad_shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
## License
|
||||||
|
This library is free software; you can redistribute it and/or modify it under
|
||||||
|
the terms of the MIT license. See [LICENSE](LICENSE) for details.
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,106 @@
|
||||||
|
/**
|
||||||
|
* Copyright (c) 2016 rxi
|
||||||
|
*
|
||||||
|
* This library is free software; you can redistribute it and/or modify it
|
||||||
|
* under the terms of the MIT license. See LICENSE for details.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef DYAD_H
|
||||||
|
#define DYAD_H
|
||||||
|
|
||||||
|
#include <stdarg.h>
|
||||||
|
|
||||||
|
#ifdef _WIN32
|
||||||
|
#include <windows.h> /* For SOCKET */
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if _WIN32
|
||||||
|
typedef SOCKET dyad_Socket;
|
||||||
|
#else
|
||||||
|
typedef int dyad_Socket;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct dyad_Stream;
|
||||||
|
typedef struct dyad_Stream dyad_Stream;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int type;
|
||||||
|
void *udata;
|
||||||
|
dyad_Stream *stream;
|
||||||
|
dyad_Stream *remote;
|
||||||
|
const char *msg;
|
||||||
|
char *data;
|
||||||
|
int size;
|
||||||
|
} dyad_Event;
|
||||||
|
|
||||||
|
typedef void (*dyad_Callback)(dyad_Event*);
|
||||||
|
typedef void (*dyad_PanicCallback)(const char*);
|
||||||
|
|
||||||
|
enum {
|
||||||
|
DYAD_EVENT_NULL,
|
||||||
|
DYAD_EVENT_DESTROY,
|
||||||
|
DYAD_EVENT_ACCEPT,
|
||||||
|
DYAD_EVENT_LISTEN,
|
||||||
|
DYAD_EVENT_CONNECT,
|
||||||
|
DYAD_EVENT_CLOSE,
|
||||||
|
DYAD_EVENT_READY,
|
||||||
|
DYAD_EVENT_DATA,
|
||||||
|
DYAD_EVENT_LINE,
|
||||||
|
DYAD_EVENT_ERROR,
|
||||||
|
DYAD_EVENT_TIMEOUT,
|
||||||
|
DYAD_EVENT_TICK
|
||||||
|
};
|
||||||
|
|
||||||
|
enum {
|
||||||
|
DYAD_STATE_CLOSED,
|
||||||
|
DYAD_STATE_CLOSING,
|
||||||
|
DYAD_STATE_CONNECTING,
|
||||||
|
DYAD_STATE_CONNECTED,
|
||||||
|
DYAD_STATE_LISTENING
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
void dyad_init(void);
|
||||||
|
void dyad_update(void);
|
||||||
|
void dyad_shutdown(void);
|
||||||
|
const char *dyad_getVersion(void);
|
||||||
|
double dyad_getTime(void);
|
||||||
|
int dyad_getStreamCount(void);
|
||||||
|
void dyad_setTickInterval(double seconds);
|
||||||
|
void dyad_setUpdateTimeout(double seconds);
|
||||||
|
dyad_PanicCallback dyad_atPanic(dyad_PanicCallback func);
|
||||||
|
|
||||||
|
dyad_Stream *dyad_newStream(void);
|
||||||
|
int dyad_listen(dyad_Stream *stream, int port);
|
||||||
|
int dyad_listenEx(dyad_Stream *stream, const char *host, int port,
|
||||||
|
int backlog);
|
||||||
|
int dyad_connect(dyad_Stream *stream, const char *host, int port);
|
||||||
|
void dyad_addListener(dyad_Stream *stream, int event,
|
||||||
|
dyad_Callback callback, void *udata);
|
||||||
|
void dyad_removeListener(dyad_Stream *stream, int event,
|
||||||
|
dyad_Callback callback, void *udata);
|
||||||
|
void dyad_removeAllListeners(dyad_Stream *stream, int event);
|
||||||
|
void dyad_end(dyad_Stream *stream);
|
||||||
|
void dyad_close(dyad_Stream *stream);
|
||||||
|
void dyad_write(dyad_Stream *stream, const void *data, int size);
|
||||||
|
void dyad_vwritef(dyad_Stream *stream, const char *fmt, va_list args);
|
||||||
|
void dyad_writef(dyad_Stream *stream, const char *fmt, ...);
|
||||||
|
void dyad_setTimeout(dyad_Stream *stream, double seconds);
|
||||||
|
void dyad_setNoDelay(dyad_Stream *stream, int opt);
|
||||||
|
int dyad_getState(dyad_Stream *stream);
|
||||||
|
const char *dyad_getAddress(dyad_Stream *stream);
|
||||||
|
int dyad_getPort(dyad_Stream *stream);
|
||||||
|
int dyad_getBytesSent(dyad_Stream *stream);
|
||||||
|
int dyad_getBytesReceived(dyad_Stream *stream);
|
||||||
|
dyad_Socket dyad_getSocket(dyad_Stream *stream);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
} // extern "C"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -53,6 +53,8 @@ extern uint8_t __config_end;
|
||||||
# define FLASH_PAGE_SIZE ((uint32_t)0x8000)
|
# define FLASH_PAGE_SIZE ((uint32_t)0x8000)
|
||||||
# elif defined(UNIT_TEST)
|
# elif defined(UNIT_TEST)
|
||||||
# define FLASH_PAGE_SIZE (0x400)
|
# define FLASH_PAGE_SIZE (0x400)
|
||||||
|
# elif defined(SIMULATOR_BUILD)
|
||||||
|
# define FLASH_PAGE_SIZE (0x400)
|
||||||
# else
|
# else
|
||||||
# error "Flash page size not defined for target."
|
# error "Flash page size not defined for target."
|
||||||
# endif
|
# endif
|
||||||
|
|
@ -87,6 +89,8 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
|
||||||
// NOP
|
// NOP
|
||||||
#elif defined(UNIT_TEST)
|
#elif defined(UNIT_TEST)
|
||||||
// NOP
|
// NOP
|
||||||
|
#elif defined(SIMULATOR_BUILD)
|
||||||
|
// NOP
|
||||||
#else
|
#else
|
||||||
# error "Unsupported CPU"
|
# error "Unsupported CPU"
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -40,7 +40,9 @@ typedef enum I2CDevice {
|
||||||
} I2CDevice;
|
} I2CDevice;
|
||||||
|
|
||||||
typedef struct i2cDevice_s {
|
typedef struct i2cDevice_s {
|
||||||
|
#if !defined(SIMULATOR_BUILD)
|
||||||
I2C_TypeDef *dev;
|
I2C_TypeDef *dev;
|
||||||
|
#endif
|
||||||
ioTag_t scl;
|
ioTag_t scl;
|
||||||
ioTag_t sda;
|
ioTag_t sda;
|
||||||
rccPeriphTag_t rcc;
|
rccPeriphTag_t rcc;
|
||||||
|
|
|
||||||
|
|
@ -145,6 +145,8 @@ uint32_t IO_EXTI_Line(IO_t io)
|
||||||
return 1 << IO_GPIOPinIdx(io);
|
return 1 << IO_GPIOPinIdx(io);
|
||||||
#elif defined(STM32F7)
|
#elif defined(STM32F7)
|
||||||
return 1 << IO_GPIOPinIdx(io);
|
return 1 << IO_GPIOPinIdx(io);
|
||||||
|
#elif defined(SIMULATOR_BUILD)
|
||||||
|
return 1;
|
||||||
#else
|
#else
|
||||||
# error "Unknown target type"
|
# error "Unknown target type"
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -88,6 +88,16 @@
|
||||||
# define IOCFG_IPU 0
|
# define IOCFG_IPU 0
|
||||||
# define IOCFG_IN_FLOATING 0
|
# define IOCFG_IN_FLOATING 0
|
||||||
|
|
||||||
|
#elif defined(SIMULATOR_BUILD)
|
||||||
|
|
||||||
|
# define IOCFG_OUT_PP 0
|
||||||
|
# define IOCFG_OUT_OD 0
|
||||||
|
# define IOCFG_AF_PP 0
|
||||||
|
# define IOCFG_AF_OD 0
|
||||||
|
# define IOCFG_IPD 0
|
||||||
|
# define IOCFG_IPU 0
|
||||||
|
# define IOCFG_IN_FLOATING 0
|
||||||
|
|
||||||
#else
|
#else
|
||||||
# warning "Unknown TARGET"
|
# warning "Unknown TARGET"
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -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,
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,60 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#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];
|
||||||
|
|
||||||
|
dyad_Stream *serv;
|
||||||
|
dyad_Stream *conn;
|
||||||
|
pthread_mutex_t txLock;
|
||||||
|
pthread_mutex_t rxLock;
|
||||||
|
bool connected;
|
||||||
|
uint16_t clientCount;
|
||||||
|
uint8_t id;
|
||||||
|
} uartPort_t;
|
||||||
|
|
||||||
|
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||||
|
|
||||||
|
// serialPort API
|
||||||
|
void tcpWrite(serialPort_t *instance, uint8_t ch);
|
||||||
|
void tcpDataIn(uartPort_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(uartPort_t *instance);
|
||||||
|
bool isTcpTransmitBufferEmpty(const serialPort_t *s);
|
||||||
|
|
||||||
|
bool tcpIsStart(void);
|
||||||
|
bool* tcpGetUsed(void);
|
||||||
|
uartPort_t* tcpGetPool(void);
|
||||||
|
|
||||||
|
|
@ -50,6 +50,11 @@ typedef uint32_t timCCR_t;
|
||||||
typedef uint32_t timCCER_t;
|
typedef uint32_t timCCER_t;
|
||||||
typedef uint32_t timSR_t;
|
typedef uint32_t timSR_t;
|
||||||
typedef uint32_t timCNT_t;
|
typedef uint32_t timCNT_t;
|
||||||
|
#elif defined(SIMULATOR_BUILD)
|
||||||
|
typedef uint32_t timCCR_t;
|
||||||
|
typedef uint32_t timCCER_t;
|
||||||
|
typedef uint32_t timSR_t;
|
||||||
|
typedef uint32_t timCNT_t;
|
||||||
#else
|
#else
|
||||||
#error "Unknown CPU defined"
|
#error "Unknown CPU defined"
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -395,6 +395,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC)
|
||||||
|
UNUSED(imuMahonyAHRSupdate);
|
||||||
|
UNUSED(useAcc);
|
||||||
|
UNUSED(useMag);
|
||||||
|
UNUSED(useYaw);
|
||||||
|
UNUSED(rawYawError);
|
||||||
|
#else
|
||||||
imuMahonyAHRSupdate(deltaT * 1e-6f,
|
imuMahonyAHRSupdate(deltaT * 1e-6f,
|
||||||
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
|
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
|
||||||
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
|
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
|
||||||
|
|
@ -402,7 +409,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
||||||
useYaw, rawYawError);
|
useYaw, rawYawError);
|
||||||
|
|
||||||
imuUpdateEulerAngles();
|
imuUpdateEulerAngles();
|
||||||
|
#endif
|
||||||
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
|
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -437,3 +444,23 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
||||||
angle = 900;
|
angle = 900;
|
||||||
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef SIMULATOR_BUILD
|
||||||
|
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
|
||||||
|
{
|
||||||
|
attitude.values.roll = roll * 10;
|
||||||
|
attitude.values.pitch = pitch * 10;
|
||||||
|
attitude.values.yaw = yaw * 10;
|
||||||
|
}
|
||||||
|
void imuSetAttitudeQuat(float w, float x, float y, float z)
|
||||||
|
{
|
||||||
|
q0 = w;
|
||||||
|
q1 = x;
|
||||||
|
q2 = y;
|
||||||
|
q3 = z;
|
||||||
|
|
||||||
|
imuComputeRotationMatrix();
|
||||||
|
imuUpdateEulerAngles();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -73,3 +73,10 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||||
|
|
||||||
void imuResetAccelerationSum(void);
|
void imuResetAccelerationSum(void);
|
||||||
void imuInit(void);
|
void imuInit(void);
|
||||||
|
|
||||||
|
#ifdef SIMULATOR_BUILD
|
||||||
|
void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg
|
||||||
|
void imuSetAttitudeQuat(float w, float x, float y, float z);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -31,6 +31,9 @@ int main(void)
|
||||||
while (true) {
|
while (true) {
|
||||||
scheduler();
|
scheduler();
|
||||||
processLoopback();
|
processLoopback();
|
||||||
|
#ifdef SIMULATOR_BUILD
|
||||||
|
delayMicroseconds_real(50); // max rate 20kHz
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -68,6 +68,10 @@
|
||||||
|
|
||||||
#define STM32F1
|
#define STM32F1
|
||||||
|
|
||||||
|
#elif defined(SITL)
|
||||||
|
|
||||||
|
// Nop
|
||||||
|
|
||||||
#else // STM32F10X
|
#else // STM32F10X
|
||||||
#error "Invalid chipset specified. Update platform.h"
|
#error "Invalid chipset specified. Update platform.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
## SITL in gazebo 8 with ArduCopterPlugin
|
||||||
|
SITL (software in the loop) simulator allows you to run betaflight/cleanflight without any hardware.
|
||||||
|
Currently only tested on Ubuntu 16.04, gcc (Ubuntu 5.4.0-6ubuntu1~16.04.4) 5.4.0 20160609.
|
||||||
|
|
||||||
|
### install gazebo 8
|
||||||
|
see here: [Installation](http://gazebosim.org/tutorials?cat=install]
|
||||||
|
|
||||||
|
### copy & modify world
|
||||||
|
for Ubunutu 16.04:
|
||||||
|
`cp /usr/share/gazebo-8/worlds/iris_arducopter_demo.world .`
|
||||||
|
|
||||||
|
change `real_time_update_rate` in `iris_arducopter_demo.world`:
|
||||||
|
`<real_time_update_rate>0</real_time_update_rate>`
|
||||||
|
to
|
||||||
|
`<real_time_update_rate>100</real_time_update_rate>`
|
||||||
|
***this MUST set to non-zero***
|
||||||
|
|
||||||
|
`100` mean what speed your computer should run in (Hz).
|
||||||
|
Faster computer can set to a higher rate.
|
||||||
|
see [here](http://gazebosim.org/tutorials?tut=modifying_world&cat=build_world#PhysicsProperties) for detail.
|
||||||
|
`max_step_size` should NOT higher than `0.0025` as I tested.
|
||||||
|
|
||||||
|
### build betaflight
|
||||||
|
run `make TARGET=SITL`
|
||||||
|
|
||||||
|
### start and run
|
||||||
|
1. start betaflight: `./obj/main/betaflight_SITL.elf`
|
||||||
|
2. start gazebo: `gazebo --verbose ./iris_arducopter_demo.world`
|
||||||
|
3. connect your transmitter and fly/test, I used a app to send `MSP_SET_RAW_RC`, code available [here](https://github.com/cs8425/msp-controller).
|
||||||
|
|
||||||
|
### note
|
||||||
|
betaflight -> gazebo `udp://127.0.0.1:9002`
|
||||||
|
gazebo -> betaflight `udp://127.0.0.1:9003`
|
||||||
|
|
||||||
|
UARTx will bind on `tcp://127.0.0.1:576x` when port been open.
|
||||||
|
|
||||||
|
`eeprom.bin`, size 8192 Byte, is for config saving.
|
||||||
|
size can be changed in `src/main/target/SITL/parameter_group.ld` >> `__FLASH_CONFIG_Size`
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
|
||||||
|
|
||||||
|
SECTIONS {
|
||||||
|
/* BLOCK: on Windows (PE) output section must be page-aligned. Use 4-byte alignment otherwise */
|
||||||
|
/* SUBALIGN: force 4-byte alignment of input sections for pg_registry.
|
||||||
|
Gcc defaults to 32 bytes; padding is then inserted between object files, breaking the init structure. */
|
||||||
|
.pg_registry BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) : SUBALIGN(4)
|
||||||
|
{
|
||||||
|
PROVIDE_HIDDEN (__pg_registry_start = . );
|
||||||
|
PROVIDE_HIDDEN (___pg_registry_start = . );
|
||||||
|
KEEP (*(.pg_registry))
|
||||||
|
KEEP (*(SORT(.pg_registry.*)))
|
||||||
|
PROVIDE_HIDDEN (__pg_registry_end = . );
|
||||||
|
PROVIDE_HIDDEN (___pg_registry_end = . );
|
||||||
|
|
||||||
|
PROVIDE_HIDDEN (__pg_resetdata_start = . );
|
||||||
|
PROVIDE_HIDDEN (___pg_resetdata_start = . );
|
||||||
|
KEEP (*(.pg_resetdata))
|
||||||
|
PROVIDE_HIDDEN (__pg_resetdata_end = . );
|
||||||
|
PROVIDE_HIDDEN (___pg_resetdata_end = . );
|
||||||
|
}
|
||||||
|
}
|
||||||
|
INSERT AFTER .text;
|
||||||
|
|
||||||
|
|
||||||
|
__FLASH_CONFIG_Size = 0x2000; /* 8kB */
|
||||||
|
SECTIONS {
|
||||||
|
/* .FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) : SUBALIGN(4)*/
|
||||||
|
.FLASH_CONFIG :
|
||||||
|
{
|
||||||
|
PROVIDE_HIDDEN (__config_start = . );
|
||||||
|
. = . + __FLASH_CONFIG_Size;
|
||||||
|
PROVIDE_HIDDEN (__config_end = . );
|
||||||
|
}
|
||||||
|
}
|
||||||
|
INSERT AFTER .bss;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,411 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <math.h>
|
||||||
|
#define printf printf
|
||||||
|
#define sprintf sprintf
|
||||||
|
|
||||||
|
#include <errno.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/dma.h"
|
||||||
|
#include "drivers/serial.h"
|
||||||
|
#include "drivers/serial_tcp.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/pwm_output.h"
|
||||||
|
#include "drivers/light_led.h"
|
||||||
|
|
||||||
|
#include "drivers/timer.h"
|
||||||
|
#include "drivers/timer_def.h"
|
||||||
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
|
||||||
|
|
||||||
|
#include "drivers/accgyro_fake.h"
|
||||||
|
#include "flight/imu.h"
|
||||||
|
|
||||||
|
#include "dyad.h"
|
||||||
|
#include "target/SITL/udplink.h"
|
||||||
|
|
||||||
|
static fdm_packet fdmPkt;
|
||||||
|
static servo_packet pwmPkt;
|
||||||
|
|
||||||
|
static struct timespec start_time;
|
||||||
|
static double simRate = 1.0;
|
||||||
|
static pthread_t tcpWorker, udpWorker;
|
||||||
|
static bool workerRunning = true;
|
||||||
|
static udpLink_t stateLink, pwmLink;
|
||||||
|
static pthread_mutex_t updateLock;
|
||||||
|
|
||||||
|
#define RAD2DEG (180.0 / M_PI)
|
||||||
|
#define ACC_SCALE (256 / 9.81)
|
||||||
|
#define GYRO_SCALE (8192 / 2000.0)
|
||||||
|
void updateState(const fdm_packet* pkt) {
|
||||||
|
static double last_timestamp = 0; // in seconds
|
||||||
|
static uint64_t last_realtime = 0; // in uS
|
||||||
|
|
||||||
|
const uint64_t realtime_now = micros64_real();
|
||||||
|
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
|
||||||
|
last_timestamp = 0;
|
||||||
|
last_realtime = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
|
||||||
|
if(deltaSim < 0) { // don't use old packet
|
||||||
|
pthread_mutex_unlock(&updateLock);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int16_t x,y,z;
|
||||||
|
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
|
||||||
|
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
|
||||||
|
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
|
||||||
|
fakeAccSet(x, y, z);
|
||||||
|
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
|
||||||
|
|
||||||
|
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
|
||||||
|
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
|
||||||
|
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
|
||||||
|
fakeGyroSet(x, y, z);
|
||||||
|
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
|
||||||
|
|
||||||
|
#if defined(SKIP_IMU_CALC)
|
||||||
|
double qw = pkt->imu_orientation_quat[0];
|
||||||
|
double qx = pkt->imu_orientation_quat[1];
|
||||||
|
double qy = pkt->imu_orientation_quat[2];
|
||||||
|
double qz = pkt->imu_orientation_quat[3];
|
||||||
|
double ysqr = qy * qy;
|
||||||
|
double xf, yf, zf;
|
||||||
|
|
||||||
|
// roll (x-axis rotation)
|
||||||
|
double t0 = +2.0 * (qw * qx + qy * qz);
|
||||||
|
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
|
||||||
|
xf = atan2(t0, t1) * RAD2DEG;
|
||||||
|
|
||||||
|
// pitch (y-axis rotation)
|
||||||
|
double t2 = +2.0 * (qw * qy - qz * qx);
|
||||||
|
t2 = t2 > 1.0 ? 1.0 : t2;
|
||||||
|
t2 = t2 < -1.0 ? -1.0 : t2;
|
||||||
|
yf = asin(t2) * RAD2DEG; // from wiki
|
||||||
|
|
||||||
|
// yaw (z-axis rotation)
|
||||||
|
double t3 = +2.0 * (qw * qz + qx * qy);
|
||||||
|
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
|
||||||
|
zf = atan2(t3, t4) * RAD2DEG;
|
||||||
|
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if(last_realtime != 0 && deltaSim < 0.02 && deltaSim > 0) { // should run faster than 50Hz
|
||||||
|
simRate = simRate * 0.99 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.01;
|
||||||
|
}
|
||||||
|
last_timestamp = pkt->timestamp;
|
||||||
|
last_realtime = micros64_real();
|
||||||
|
// printf("simRate = %lf\n", simRate);
|
||||||
|
|
||||||
|
pthread_mutex_unlock(&updateLock); // can send PWM output now
|
||||||
|
}
|
||||||
|
|
||||||
|
static void* udpThread(void* data) {
|
||||||
|
UNUSED(data);
|
||||||
|
int n = 0;
|
||||||
|
|
||||||
|
while (workerRunning) {
|
||||||
|
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
|
||||||
|
if(n == sizeof(fdm_packet)) {
|
||||||
|
// printf("[data]new fdm %d\n", n);
|
||||||
|
updateState(&fdmPkt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("udpThread end!!\n");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void* tcpThread(void* data) {
|
||||||
|
UNUSED(data);
|
||||||
|
|
||||||
|
dyad_init();
|
||||||
|
dyad_setTickInterval(0.2f);
|
||||||
|
dyad_setUpdateTimeout(0.5f);
|
||||||
|
|
||||||
|
while (workerRunning) {
|
||||||
|
dyad_update();
|
||||||
|
}
|
||||||
|
|
||||||
|
dyad_shutdown();
|
||||||
|
printf("tcpThread end!!\n");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// system
|
||||||
|
void systemInit(void) {
|
||||||
|
int ret;
|
||||||
|
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||||
|
printf("[system]Init...\n");
|
||||||
|
|
||||||
|
SystemCoreClock = 500 * 1e6; // fake 500MHz
|
||||||
|
FLASH_Unlock();
|
||||||
|
|
||||||
|
if (pthread_mutex_init(&updateLock, NULL) != 0) {
|
||||||
|
printf("Create updateLock error!\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
|
||||||
|
if(ret != 0) {
|
||||||
|
printf("Create tcpWorker error!\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
|
||||||
|
printf("init PwnOut UDP link...%d\n", ret);
|
||||||
|
|
||||||
|
ret = udpInit(&stateLink, NULL, 9003, true);
|
||||||
|
printf("start UDP server...%d\n", ret);
|
||||||
|
|
||||||
|
ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
|
||||||
|
if(ret != 0) {
|
||||||
|
printf("Create udpWorker error!\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void systemReset(void){
|
||||||
|
printf("[system]Reset!\n");
|
||||||
|
workerRunning = false;
|
||||||
|
pthread_join(tcpWorker, NULL);
|
||||||
|
pthread_join(udpWorker, NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
void systemResetToBootloader(void) {
|
||||||
|
printf("[system]ResetToBootloader!\n");
|
||||||
|
workerRunning = false;
|
||||||
|
pthread_join(tcpWorker, NULL);
|
||||||
|
pthread_join(udpWorker, NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// drivers/light_led.c
|
||||||
|
void ledInit(const statusLedConfig_t *statusLedConfig) {
|
||||||
|
UNUSED(statusLedConfig);
|
||||||
|
printf("[led]Init...\n");
|
||||||
|
}
|
||||||
|
void timerInit(void) {
|
||||||
|
printf("[timer]Init...\n");
|
||||||
|
}
|
||||||
|
void timerStart(void) {
|
||||||
|
}
|
||||||
|
void failureMode(failureMode_e mode) {
|
||||||
|
printf("[failureMode]!!! %d\n", mode);
|
||||||
|
while(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Time part
|
||||||
|
// Thanks ArduPilot
|
||||||
|
uint64_t micros64_real() {
|
||||||
|
struct timespec ts;
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
|
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
|
||||||
|
(start_time.tv_sec +
|
||||||
|
(start_time.tv_nsec*1.0e-9)));
|
||||||
|
}
|
||||||
|
uint64_t millis64_real() {
|
||||||
|
struct timespec ts;
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
|
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
|
||||||
|
(start_time.tv_sec +
|
||||||
|
(start_time.tv_nsec*1.0e-9)));
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t micros64() {
|
||||||
|
static uint64_t last_time = 0;
|
||||||
|
uint64_t now = micros64_real();
|
||||||
|
uint64_t out = last_time + ((now - last_time) * simRate);
|
||||||
|
last_time = out;
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
uint64_t millis64() {
|
||||||
|
static uint64_t last_time = 0;
|
||||||
|
uint64_t now = millis64_real();
|
||||||
|
uint64_t out = last_time + ((now - last_time) * simRate);
|
||||||
|
last_time = out;
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t micros(void) {
|
||||||
|
return micros64() & 0xFFFFFFFF;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t millis(void) {
|
||||||
|
return millis64() & 0xFFFFFFFF;
|
||||||
|
}
|
||||||
|
|
||||||
|
void microsleep(uint32_t usec) {
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 0;
|
||||||
|
ts.tv_nsec = usec*1000UL;
|
||||||
|
while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
|
||||||
|
}
|
||||||
|
void delayMicroseconds(uint32_t us) {
|
||||||
|
microsleep(us / simRate);
|
||||||
|
}
|
||||||
|
void delayMicroseconds_real(uint32_t us) {
|
||||||
|
microsleep(us);
|
||||||
|
}
|
||||||
|
void delay(uint32_t ms) {
|
||||||
|
uint64_t start = millis64();
|
||||||
|
|
||||||
|
while ((millis64() - start) < ms) {
|
||||||
|
microsleep(1000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// PWM part
|
||||||
|
bool pwmMotorsEnabled = false;
|
||||||
|
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
|
||||||
|
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
|
||||||
|
|
||||||
|
// real value to send
|
||||||
|
static uint16_t motorsPwm[MAX_SUPPORTED_MOTORS];
|
||||||
|
static uint16_t servosPwm[MAX_SUPPORTED_SERVOS];
|
||||||
|
static uint16_t idlePulse;
|
||||||
|
|
||||||
|
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
|
||||||
|
UNUSED(motorConfig);
|
||||||
|
UNUSED(motorCount);
|
||||||
|
|
||||||
|
idlePulse = _idlePulse;
|
||||||
|
|
||||||
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||||
|
motors[motorIndex].enabled = true;
|
||||||
|
}
|
||||||
|
pwmMotorsEnabled = true;
|
||||||
|
}
|
||||||
|
void servoDevInit(const servoDevConfig_t *servoConfig) {
|
||||||
|
UNUSED(servoConfig);
|
||||||
|
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||||
|
servos[servoIndex].enabled = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pwmOutputPort_t *pwmGetMotors(void) {
|
||||||
|
return motors;
|
||||||
|
}
|
||||||
|
bool pwmAreMotorsEnabled(void) {
|
||||||
|
return pwmMotorsEnabled;
|
||||||
|
}
|
||||||
|
void pwmWriteMotor(uint8_t index, uint16_t value) {
|
||||||
|
motorsPwm[index] = value - idlePulse;
|
||||||
|
}
|
||||||
|
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
|
||||||
|
UNUSED(motorCount);
|
||||||
|
pwmMotorsEnabled = false;
|
||||||
|
}
|
||||||
|
void pwmCompleteMotorUpdate(uint8_t motorCount) {
|
||||||
|
UNUSED(motorCount);
|
||||||
|
// send to simulator
|
||||||
|
// for gazebo8 ArduCopterPlugin remap, range = [0.0, 1.0]
|
||||||
|
pwmPkt.motor_speed[3] = motorsPwm[0] / 1000.0f;
|
||||||
|
pwmPkt.motor_speed[0] = motorsPwm[1] / 1000.0f;
|
||||||
|
pwmPkt.motor_speed[1] = motorsPwm[2] / 1000.0f;
|
||||||
|
pwmPkt.motor_speed[2] = motorsPwm[3] / 1000.0f;
|
||||||
|
|
||||||
|
// 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]);
|
||||||
|
}
|
||||||
|
void pwmWriteServo(uint8_t index, uint16_t value) {
|
||||||
|
servosPwm[index] = value;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ADC part
|
||||||
|
uint16_t adcGetChannel(uint8_t channel) {
|
||||||
|
UNUSED(channel);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// stack part
|
||||||
|
char _estack;
|
||||||
|
char _Min_Stack_Size;
|
||||||
|
|
||||||
|
// fake EEPROM
|
||||||
|
extern uint8_t __config_start;
|
||||||
|
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) {
|
||||||
|
printf("[FLASH_Unlock] eepromFd != NULL\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// open or create
|
||||||
|
eepromFd = fopen(EEPROM_FILE,"r+");
|
||||||
|
if(eepromFd != NULL) {
|
||||||
|
long lSize;
|
||||||
|
int c;
|
||||||
|
|
||||||
|
// obtain file size:
|
||||||
|
fseek(eepromFd , 0 , SEEK_END);
|
||||||
|
lSize = ftell(eepromFd);
|
||||||
|
rewind(eepromFd);
|
||||||
|
|
||||||
|
printf("[FLASH_Unlock]size = %ld\n", lSize);
|
||||||
|
for(unsigned int i=0; i<((uintptr_t)&__FLASH_CONFIG_Size); i++){
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void FLASH_Lock(void) {
|
||||||
|
// flush & close
|
||||||
|
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);
|
||||||
|
fclose(eepromFd);
|
||||||
|
eepromFd = NULL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
FLASH_Status FLASH_ErasePage(uint32_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) {
|
||||||
|
*((uint32_t*)(uintptr_t)addr) = Data;
|
||||||
|
// printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
|
||||||
|
return FLASH_COMPLETE;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,234 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// SITL (software in the loop) simulator
|
||||||
|
#define TARGET_BOARD_IDENTIFIER "SITL"
|
||||||
|
|
||||||
|
// use simulatior's attitude directly
|
||||||
|
#define SKIP_IMU_CALC
|
||||||
|
|
||||||
|
// file name to save config
|
||||||
|
#define EEPROM_FILENAME "eeprom.bin"
|
||||||
|
|
||||||
|
#define SIMULATOR_BUILD
|
||||||
|
|
||||||
|
#define U_ID_0 0
|
||||||
|
#define U_ID_1 1
|
||||||
|
#define U_ID_2 2
|
||||||
|
|
||||||
|
#define ACC
|
||||||
|
#define USE_FAKE_ACC
|
||||||
|
|
||||||
|
#define GYRO
|
||||||
|
#define USE_FAKE_GYRO
|
||||||
|
|
||||||
|
#define MAG
|
||||||
|
#define USE_FAKE_MAG
|
||||||
|
|
||||||
|
#define BARO
|
||||||
|
#define USE_FAKE_BARO
|
||||||
|
|
||||||
|
#define USABLE_TIMER_CHANNEL_COUNT 0
|
||||||
|
|
||||||
|
#define USE_UART1
|
||||||
|
#define USE_UART2
|
||||||
|
#define USE_UART3
|
||||||
|
#define USE_UART4
|
||||||
|
#define USE_UART5
|
||||||
|
#define USE_UART6
|
||||||
|
#define USE_UART7
|
||||||
|
#define USE_UART8
|
||||||
|
|
||||||
|
//#define USE_SOFTSERIAL1
|
||||||
|
//#define USE_SOFTSERIAL2
|
||||||
|
|
||||||
|
#define SERIAL_PORT_COUNT 8
|
||||||
|
|
||||||
|
#define DEFAULT_RX_FEATURE FEATURE_RX_MSP
|
||||||
|
#define DEFAULT_FEATURES (FEATURE_GPS | FEATURE_TELEMETRY)
|
||||||
|
|
||||||
|
#define USE_PARAMETER_GROUPS
|
||||||
|
|
||||||
|
#undef STACK_CHECK // I think SITL don't need this
|
||||||
|
#undef USE_DASHBOARD
|
||||||
|
#undef TELEMETRY_LTM
|
||||||
|
#undef USE_ADC
|
||||||
|
#undef USE_VCP
|
||||||
|
#undef USE_PPM
|
||||||
|
#undef USE_PWM
|
||||||
|
#undef SERIAL_RX
|
||||||
|
#undef USE_SERIALRX_CRSF
|
||||||
|
#undef USE_SERIALRX_IBUS
|
||||||
|
#undef USE_SERIALRX_SBUS
|
||||||
|
#undef USE_SERIALRX_SPEKTRUM
|
||||||
|
#undef USE_SERIALRX_SUMD
|
||||||
|
#undef USE_SERIALRX_SUMH
|
||||||
|
#undef USE_SERIALRX_XBUS
|
||||||
|
#undef LED_STRIP
|
||||||
|
#undef TELEMETRY_FRSKY
|
||||||
|
#undef TELEMETRY_HOTT
|
||||||
|
#undef TELEMETRY_SMARTPORT
|
||||||
|
#undef TELEMETRY_MAVLINK
|
||||||
|
#undef USE_RESOURCE_MGMT
|
||||||
|
#undef CMS
|
||||||
|
#undef TELEMETRY_CRSF
|
||||||
|
#undef TELEMETRY_IBUS
|
||||||
|
#undef TELEMETRY_JETIEXBUS
|
||||||
|
#undef TELEMETRY_SRXL
|
||||||
|
#undef USE_SERIALRX_JETIEXBUS
|
||||||
|
#undef VTX_COMMON
|
||||||
|
#undef VTX_CONTROL
|
||||||
|
#undef VTX_SMARTAUDIO
|
||||||
|
#undef VTX_TRAMP
|
||||||
|
|
||||||
|
#undef USE_I2C
|
||||||
|
#undef USE_SPI
|
||||||
|
|
||||||
|
#define FLASH_SIZE 2048
|
||||||
|
|
||||||
|
# define DEFIO_PORT_USED_COUNT 0
|
||||||
|
# define DEFIO_PORT_USED_LIST /* empty */
|
||||||
|
# define DEFIO_PORT_OFFSET_LIST /* empty */
|
||||||
|
|
||||||
|
#define LED_STRIP_TIMER 1
|
||||||
|
#define SOFTSERIAL_1_TIMER 2
|
||||||
|
#define SOFTSERIAL_2_TIMER 3
|
||||||
|
|
||||||
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
#define TARGET_IO_PORTB 0xffff
|
||||||
|
#define TARGET_IO_PORTC 0xffff
|
||||||
|
|
||||||
|
#define WS2811_DMA_TC_FLAG (void *)1
|
||||||
|
#define WS2811_DMA_HANDLER_IDENTIFER 0
|
||||||
|
|
||||||
|
|
||||||
|
// belows are internal stuff
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
uint32_t SystemCoreClock;
|
||||||
|
|
||||||
|
#define UNUSED(x) (void)(x)
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
Mode_TEST = 0x0,
|
||||||
|
Mode_Out_PP = 0x10
|
||||||
|
} GPIO_Mode;
|
||||||
|
|
||||||
|
typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus;
|
||||||
|
typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
|
||||||
|
typedef enum {TEST_IRQ = 0 } IRQn_Type;
|
||||||
|
typedef enum {
|
||||||
|
EXTI_Trigger_Rising = 0x08,
|
||||||
|
EXTI_Trigger_Falling = 0x0C,
|
||||||
|
EXTI_Trigger_Rising_Falling = 0x10
|
||||||
|
} EXTITrigger_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t IDR;
|
||||||
|
uint32_t ODR;
|
||||||
|
uint32_t BSRR;
|
||||||
|
uint32_t BRR;
|
||||||
|
} GPIO_TypeDef;
|
||||||
|
|
||||||
|
#define GPIOA_BASE (0x0000)
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} TIM_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} TIM_OCInitTypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
void* test;
|
||||||
|
} DMA_TypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
void* test;
|
||||||
|
} DMA_Channel_TypeDef;
|
||||||
|
|
||||||
|
uint8_t DMA_GetFlagStatus(void *);
|
||||||
|
void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState );
|
||||||
|
void DMA_ClearFlag(uint32_t);
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} SPI_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} USART_TypeDef;
|
||||||
|
|
||||||
|
#define USART1 ((USART_TypeDef *)0x0001)
|
||||||
|
#define USART2 ((USART_TypeDef *)0x0002)
|
||||||
|
#define USART3 ((USART_TypeDef *)0x0003)
|
||||||
|
#define USART4 ((USART_TypeDef *)0x0004)
|
||||||
|
#define USART5 ((USART_TypeDef *)0x0005)
|
||||||
|
#define USART6 ((USART_TypeDef *)0x0006)
|
||||||
|
#define USART7 ((USART_TypeDef *)0x0007)
|
||||||
|
#define USART8 ((USART_TypeDef *)0x0008)
|
||||||
|
|
||||||
|
#define UART4 ((USART_TypeDef *)0x0004)
|
||||||
|
#define UART5 ((USART_TypeDef *)0x0005)
|
||||||
|
#define UART7 ((USART_TypeDef *)0x0007)
|
||||||
|
#define UART8 ((USART_TypeDef *)0x0008)
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} I2C_TypeDef;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
FLASH_BUSY = 1,
|
||||||
|
FLASH_ERROR_PG,
|
||||||
|
FLASH_ERROR_WRP,
|
||||||
|
FLASH_COMPLETE,
|
||||||
|
FLASH_TIMEOUT
|
||||||
|
} FLASH_Status;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double timestamp; // in seconds
|
||||||
|
double imu_angular_velocity_rpy[3]; // range: +/- 8192; +/- 2000 deg/se
|
||||||
|
double imu_linear_acceleration_xyz[3]; // sim 1G = 9.81 -> FC 1G = 256
|
||||||
|
double imu_orientation_quat[4];
|
||||||
|
double velocity_xyz[3];
|
||||||
|
double position_xyz[3];
|
||||||
|
} fdm_packet;
|
||||||
|
typedef struct {
|
||||||
|
float motor_speed[4]; // [0.0, 1.0]
|
||||||
|
} servo_packet;
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
uint64_t micros64_real();
|
||||||
|
uint64_t millis64_real();
|
||||||
|
void delayMicroseconds_real(uint32_t us);
|
||||||
|
|
||||||
|
|
@ -0,0 +1,9 @@
|
||||||
|
SITL_TARGETS += $(TARGET)
|
||||||
|
#FEATURES += SDCARD VCP
|
||||||
|
|
||||||
|
TARGET_SRC = \
|
||||||
|
drivers/accgyro_fake.c \
|
||||||
|
drivers/barometer_fake.c \
|
||||||
|
drivers/compass_fake.c \
|
||||||
|
drivers/serial_tcp.c
|
||||||
|
|
||||||
|
|
@ -0,0 +1,67 @@
|
||||||
|
/**
|
||||||
|
* Copyright (c) 2017 cs8425
|
||||||
|
*
|
||||||
|
* This library is free software; you can redistribute it and/or modify it
|
||||||
|
* under the terms of the MIT license.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include <fcntl.h>
|
||||||
|
|
||||||
|
#include "udplink.h"
|
||||||
|
|
||||||
|
int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) {
|
||||||
|
int one = 1;
|
||||||
|
|
||||||
|
if((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
|
||||||
|
setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind
|
||||||
|
fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock
|
||||||
|
|
||||||
|
link->isServer = isServer;
|
||||||
|
memset(&link->si, 0, sizeof(link->si));
|
||||||
|
link->si.sin_family = AF_INET;
|
||||||
|
link->si.sin_port = htons(port);
|
||||||
|
link->port = port;
|
||||||
|
|
||||||
|
if(addr == NULL) {
|
||||||
|
link->si.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||||
|
}else{
|
||||||
|
link->si.sin_addr.s_addr = inet_addr(addr);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(isServer) {
|
||||||
|
if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int udpSend(udpLink_t* link, const void* data, size_t size) {
|
||||||
|
return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si));
|
||||||
|
}
|
||||||
|
|
||||||
|
int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms) {
|
||||||
|
fd_set fds;
|
||||||
|
struct timeval tv;
|
||||||
|
|
||||||
|
FD_ZERO(&fds);
|
||||||
|
FD_SET(link->fd, &fds);
|
||||||
|
|
||||||
|
tv.tv_sec = timeout_ms / 1000;
|
||||||
|
tv.tv_usec = (timeout_ms % 1000) * 1000UL;
|
||||||
|
|
||||||
|
if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
socklen_t len;
|
||||||
|
int ret;
|
||||||
|
ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
/**
|
||||||
|
* Copyright (c) 2017 cs8425
|
||||||
|
*
|
||||||
|
* This library is free software; you can redistribute it and/or modify it
|
||||||
|
* under the terms of the MIT license.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef __UDPLINK_H
|
||||||
|
#define __UDPLINK_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int fd;
|
||||||
|
struct sockaddr_in si;
|
||||||
|
struct sockaddr_in recv;
|
||||||
|
int port;
|
||||||
|
char* addr;
|
||||||
|
bool isServer;
|
||||||
|
} udpLink_t;
|
||||||
|
|
||||||
|
int udpInit(udpLink_t* link, const char* addr, int port, bool isServer);
|
||||||
|
int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms);
|
||||||
|
int udpSend(udpLink_t* link, const void* data, size_t size);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
} // extern "C"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
Loading…
Reference in New Issue