From c6110f8b1305b3c21a20a8fd8329696a58866dfc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 17 Apr 2017 17:15:42 -0700 Subject: [PATCH 01/29] aerofc: Move GPS to UART7 The UART3 also have the I2C bus 2 functions so moving GPS to UART7 to have one additional I2C. To keep GPS working is also necessary update the FPGA RTL to version 0xC1 or higher. --- ROMFS/px4fmu_common/init.d/rcS | 2 +- nuttx-configs/aerofc-v1/include/board.h | 8 ++-- nuttx-configs/aerofc-v1/nsh/defconfig | 43 ++++++++++++--------- src/drivers/boards/aerofc-v1/board_config.h | 4 +- 4 files changed, 31 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3507908cb8da..f506205c35ef 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -648,7 +648,7 @@ then if ver hwcmp AEROFC_V1 then - set MAVLINK_F "-r 1200 -d /dev/ttyS4" + set MAVLINK_F "-r 1200 -d /dev/ttyS3" fi fi diff --git a/nuttx-configs/aerofc-v1/include/board.h b/nuttx-configs/aerofc-v1/include/board.h index 3ce4cfe7d2df..f895535d47c7 100644 --- a/nuttx-configs/aerofc-v1/include/board.h +++ b/nuttx-configs/aerofc-v1/include/board.h @@ -229,10 +229,10 @@ #define GPIO_USART2_RTS 0 // unused // GPS -#define GPIO_USART3_TX GPIO_USART3_TX_1 -#define GPIO_USART3_RX GPIO_USART3_RX_1 -#define GPIO_USART3_CTS 0 // unused -#define GPIO_USART3_RTS 0 // unused +#define GPIO_UART7_TX GPIO_UART7_TX_1 +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_CTS 0 // unused +#define GPIO_UART7_RTS 0 // unused // RC #define GPIO_UART4_TX GPIO_UART4_TX_1 diff --git a/nuttx-configs/aerofc-v1/nsh/defconfig b/nuttx-configs/aerofc-v1/nsh/defconfig index f65d1f738c93..7634826ff442 100644 --- a/nuttx-configs/aerofc-v1/nsh/defconfig +++ b/nuttx-configs/aerofc-v1/nsh/defconfig @@ -421,11 +421,11 @@ CONFIG_STM32_TIM11=y # CONFIG_STM32_TIM14 is not set CONFIG_STM32_USART1=y CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y +# CONFIG_STM32_USART3 is not set CONFIG_STM32_UART4=y CONFIG_STM32_UART5=y CONFIG_STM32_USART6=y -# CONFIG_STM32_UART7 is not set +CONFIG_STM32_UART7=y # CONFIG_STM32_UART8 is not set # CONFIG_STM32_IWDG is not set CONFIG_STM32_WWDG=y @@ -487,10 +487,10 @@ CONFIG_STM32_USART2_SERIALDRIVER=y # CONFIG_STM32_USART2_1WIREDRIVER is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y -CONFIG_STM32_USART3_SERIALDRIVER=y +# CONFIG_STM32_USART3_SERIALDRIVER is not set # CONFIG_STM32_USART3_1WIREDRIVER is not set # CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y +# CONFIG_USART3_RXDMA is not set CONFIG_STM32_UART4_SERIALDRIVER=y # CONFIG_STM32_UART4_1WIREDRIVER is not set # CONFIG_UART4_RS485 is not set @@ -503,6 +503,10 @@ CONFIG_STM32_USART6_SERIALDRIVER=y # CONFIG_STM32_USART6_1WIREDRIVER is not set # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y +CONFIG_STM32_UART7_SERIALDRIVER=y +# CONFIG_STM32_UART7_1WIREDRIVER is not set +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y # # Serial Driver Configuration @@ -891,14 +895,14 @@ CONFIG_SERIAL_CONSOLE=y CONFIG_UART4_SERIALDRIVER=y CONFIG_UART5_SERIALDRIVER=y # CONFIG_UART6_SERIALDRIVER is not set -# CONFIG_UART7_SERIALDRIVER is not set +CONFIG_UART7_SERIALDRIVER=y # CONFIG_UART8_SERIALDRIVER is not set # CONFIG_SCI0_SERIALDRIVER is not set # CONFIG_SCI1_SERIALDRIVER is not set # CONFIG_USART0_SERIALDRIVER is not set CONFIG_USART1_SERIALDRIVER=y CONFIG_USART2_SERIALDRIVER=y -CONFIG_USART3_SERIALDRIVER=y +# CONFIG_USART3_SERIALDRIVER is not set # CONFIG_USART4_SERIALDRIVER is not set # CONFIG_USART5_SERIALDRIVER is not set CONFIG_USART6_SERIALDRIVER=y @@ -918,6 +922,7 @@ CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set CONFIG_USART6_SERIAL_CONSOLE=y +# CONFIG_UART7_SERIAL_CONSOLE is not set # CONFIG_OTHER_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set @@ -947,19 +952,6 @@ CONFIG_USART2_2STOP=0 # CONFIG_USART2_OFLOWCONTROL is not set # CONFIG_USART2_DMA is not set -# -# USART3 Configuration -# -CONFIG_USART3_RXBUFSIZE=300 -CONFIG_USART3_TXBUFSIZE=300 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_BITS=8 -CONFIG_USART3_PARITY=0 -CONFIG_USART3_2STOP=0 -# CONFIG_USART3_IFLOWCONTROL is not set -# CONFIG_USART3_OFLOWCONTROL is not set -# CONFIG_USART3_DMA is not set - # # UART4 Configuration # @@ -1007,6 +999,19 @@ CONFIG_USART6_2STOP=0 # CONFIG_DRIVERS_WIRELESS is not set # CONFIG_DRIVERS_CONTACTLESS is not set +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set +# CONFIG_UART7_DMA is not set + # # System Logging # diff --git a/src/drivers/boards/aerofc-v1/board_config.h b/src/drivers/boards/aerofc-v1/board_config.h index 8da3ff88265b..9f76b381eed5 100644 --- a/src/drivers/boards/aerofc-v1/board_config.h +++ b/src/drivers/boards/aerofc-v1/board_config.h @@ -116,12 +116,12 @@ /* * GPS */ -#define GPS_DEFAULT_UART_PORT "/dev/ttyS2" +#define GPS_DEFAULT_UART_PORT "/dev/ttyS5" /* * RC Serial port */ -#define RC_SERIAL_PORT "/dev/ttyS3" +#define RC_SERIAL_PORT "/dev/ttyS2" /* No invert support */ #define INVERT_RC_INPUT(_invert_true) while(0) From b241f72254966753b5dd8ace1594faa7a5510723 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 17 Apr 2017 17:17:36 -0700 Subject: [PATCH 02/29] nuttx-configs: aerofc: Remove GPIO_I2C*_S**_GPIO Nothing use this define right now so lets remove it. Several other boards also have this defines that can also be removed. --- nuttx-configs/aerofc-v1/include/board.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/nuttx-configs/aerofc-v1/include/board.h b/nuttx-configs/aerofc-v1/include/board.h index f895535d47c7..5d7a5600c4fb 100644 --- a/nuttx-configs/aerofc-v1/include/board.h +++ b/nuttx-configs/aerofc-v1/include/board.h @@ -265,13 +265,9 @@ */ #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 -#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) -#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) #define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 -#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) -#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) /* * SPI From 79e93ffa13d2aa990b53fdde45a1e4c33fce8417 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 17 Apr 2017 17:19:32 -0700 Subject: [PATCH 03/29] aerofc: Enable I2C bus 2 Now that UART3 is no longer in use we can use this I2C bus. --- nuttx-configs/aerofc-v1/include/board.h | 3 +++ nuttx-configs/aerofc-v1/nsh/defconfig | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/nuttx-configs/aerofc-v1/include/board.h b/nuttx-configs/aerofc-v1/include/board.h index 5d7a5600c4fb..02f1f6d85df9 100644 --- a/nuttx-configs/aerofc-v1/include/board.h +++ b/nuttx-configs/aerofc-v1/include/board.h @@ -266,6 +266,9 @@ #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 + #define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 diff --git a/nuttx-configs/aerofc-v1/nsh/defconfig b/nuttx-configs/aerofc-v1/nsh/defconfig index 7634826ff442..26d7f10891ee 100644 --- a/nuttx-configs/aerofc-v1/nsh/defconfig +++ b/nuttx-configs/aerofc-v1/nsh/defconfig @@ -391,7 +391,7 @@ CONFIG_STM32_DMA2=y # CONFIG_STM32_FSMC is not set # CONFIG_STM32_HASH is not set CONFIG_STM32_I2C1=y -# CONFIG_STM32_I2C2 is not set +CONFIG_STM32_I2C2=y CONFIG_STM32_I2C3=y CONFIG_STM32_OTGFS=y # CONFIG_STM32_OTGHS is not set From afafb13e3b7c5664cba88d5baada9857a86929a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Thu, 4 May 2017 13:12:06 -0700 Subject: [PATCH 04/29] aerofc_adc: Add support to use others I2C besides PX4_I2C_BUS_EXPANSION --- src/drivers/aerofc_adc/aerofc_adc.cpp | 86 +++++++++++++++++++++++---- 1 file changed, 74 insertions(+), 12 deletions(-) diff --git a/src/drivers/aerofc_adc/aerofc_adc.cpp b/src/drivers/aerofc_adc/aerofc_adc.cpp index 4f5c36b2b315..a3e57e2c8c57 100644 --- a/src/drivers/aerofc_adc/aerofc_adc.cpp +++ b/src/drivers/aerofc_adc/aerofc_adc.cpp @@ -54,6 +54,27 @@ // 10Hz #define CYCLE_TICKS_DELAY MSEC2TICK(100) +enum AEROFC_ADC_BUS { + AEROFC_ADC_BUS_ALL = 0, + AEROFC_ADC_BUS_I2C_INTERNAL, + AEROFC_ADC_BUS_I2C_EXTERNAL +}; + +static struct aerofc_adc_bus_option { + enum AEROFC_ADC_BUS busid; + uint8_t busnum; +} bus_options[] = { +#ifdef PX4_I2C_BUS_EXPANSION + { AEROFC_ADC_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION1 + { AEROFC_ADC_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION1 }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { AEROFC_ADC_BUS_I2C_INTERNAL, PX4_I2C_BUS_ONBOARD }, +#endif +}; + extern "C" { __EXPORT int aerofc_adc_main(int argc, char *argv[]); } class AEROFC_ADC : public device::I2C @@ -105,38 +126,80 @@ static void test() exit(0); } +static void help() +{ + printf("missing command: try 'start' or 'test'\n"); + printf("options:\n"); + printf(" -I only internal I2C bus\n"); + printf(" -X only external I2C bus\n"); +} + int aerofc_adc_main(int argc, char *argv[]) { - if (argc < 2) { - warn("Missing action "); + int ch; + enum AEROFC_ADC_BUS busid = AEROFC_ADC_BUS_ALL; + + while ((ch = getopt(argc, argv, "XI")) != EOF) { + switch (ch) { + case 'X': + busid = AEROFC_ADC_BUS_I2C_EXTERNAL; + break; + + case 'I': + busid = AEROFC_ADC_BUS_I2C_INTERNAL; + break; + + default: + help(); + return -1; + } + } + + if (optind >= argc) { + help(); return PX4_ERROR; } - if (!strcmp(argv[1], "start")) { + const char *verb = argv[optind]; + + if (!strcmp(verb, "start")) { if (instance) { warn("AEROFC_ADC was already started"); return PX4_OK; } - instance = new AEROFC_ADC(PX4_I2C_BUS_EXPANSION); + for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) { + if (busid != AEROFC_ADC_BUS_ALL && busid != bus_options[i].busid) { + continue; + } - if (!instance) { - warn("No memory to instance AEROFC_ADC"); - return PX4_ERROR; - } + instance = new AEROFC_ADC(bus_options[i].busnum); + + if (!instance) { + warn("No memory to instance AEROFC_ADC"); + return PX4_ERROR; + } - if (instance->init() != PX4_OK) { + if (instance->init() == PX4_OK) { + break; + } + + warn("AEROFC_ADC not found on busnum=%u", bus_options[i].busnum); delete instance; instance = nullptr; - warn("AEROFC_ADC failed to init"); + } + + if (!instance) { + warn("AEROFC_ADC not found"); return PX4_ERROR; } - } else if (!strcmp(argv[1], "test")) { + } else if (!strcmp(verb, "test")) { test(); } else { warn("Action not supported"); + help(); return PX4_ERROR; } @@ -199,7 +262,6 @@ int AEROFC_ADC::probe() return PX4_OK; error: - warn("AEROFC_ADC not found"); return -EIO; } From 20822ec189e3fc19c072e74b81cc0ddcfe676c2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Thu, 4 May 2017 16:19:46 -0700 Subject: [PATCH 05/29] ll40ls: Refactor interface(PWM and I2C) selection and allow probe in more I2C buses --- src/drivers/ll40ls/LidarLite.h | 2 + src/drivers/ll40ls/LidarLiteI2C.cpp | 5 + src/drivers/ll40ls/LidarLiteI2C.h | 3 +- src/drivers/ll40ls/LidarLitePWM.cpp | 5 + src/drivers/ll40ls/LidarLitePWM.h | 2 + src/drivers/ll40ls/ll40ls.cpp | 394 +++++++++++----------------- 6 files changed, 166 insertions(+), 245 deletions(-) diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index 169f0dfc77ba..2300e0cba7ce 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -80,6 +80,8 @@ class LidarLite */ virtual void print_registers() = 0; + virtual const char *get_dev_name() = 0; + protected: /** * Set the min and max distance thresholds if you want the end points of the sensors diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 230c1064ee19..6078f12741e9 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -564,3 +564,8 @@ void LidarLiteI2C::print_info() printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); } + +const char *LidarLiteI2C::get_dev_name() +{ + return get_devname(); +} diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index ea9b6c03027a..b031cf253040 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -54,7 +54,6 @@ /* Configuration Constants */ -#define LL40LS_BUS PX4_I2C_BUS_EXPANSION #define LL40LS_BASEADDR 0x62 /* 7-bit address */ #define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ @@ -90,6 +89,8 @@ class LidarLiteI2C : public LidarLite, public device::I2C */ void print_registers() override; + const char *get_dev_name() override; + protected: int probe(); int read_reg(uint8_t reg, uint8_t &val); diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 63a11c8ad901..5713322b280a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -282,3 +282,8 @@ int LidarLitePWM::reset_sensor() ::close(fd); return ret; } + +const char *LidarLitePWM::get_dev_name() +{ + return get_devname(); +} diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 4e51f2d5fa90..503861d8a130 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -94,6 +94,8 @@ class LidarLitePWM : public LidarLite, public device::CDev */ static void cycle_trampoline(void *arg); + const char *get_dev_name() override; + protected: int measure() override; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8af82a18745a..b33e18946265 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -54,10 +54,31 @@ # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" -#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" #define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" +enum LL40LS_BUS { + LL40LS_BUS_I2C_ALL = 0, + LL40LS_BUS_I2C_INTERNAL, + LL40LS_BUS_I2C_EXTERNAL, + LL40LS_BUS_PWM +}; + +static struct ll40ls_bus_option { + enum LL40LS_BUS busid; + const char *devname; + uint8_t busnum; +} bus_options[] = { +#ifdef PX4_I2C_BUS_EXPANSION + { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext", PX4_I2C_BUS_EXPANSION }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION1 + { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext1", PX4_I2C_BUS_EXPANSION1 }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { LL40LS_BUS_I2C_INTERNAL, "/dev/ll40ls_int", PX4_I2C_BUS_ONBOARD }, +#endif +}; + /* * Driver 'main' command. */ @@ -70,210 +91,97 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); namespace ll40ls { -LidarLiteI2C *g_dev_int; -LidarLiteI2C *g_dev_ext; -LidarLitePWM *g_dev_pwm; - -LidarLite *get_dev(const bool use_i2c, const int bus); -void start(const bool use_i2c, const int bus); -void stop(const bool use_i2c, const int bus); -void test(const bool use_i2c, const int bus); -void reset(const bool use_i2c, const int bus); -void info(const bool use_i2c, const int bus); -void regdump(const bool use_i2c, const int bus); -void usage(); - -/** - * Get the correct device pointer - */ -LidarLite *get_dev(const bool use_i2c, const int bus) -{ - LidarLite *g_dev = nullptr; - - if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int); - - if (g_dev == nullptr) { - errx(1, "i2c driver not running"); - } - - } else { - g_dev = static_cast(g_dev_pwm); - - if (g_dev == nullptr) { - errx(1, "pwm driver not running"); - } - } +LidarLite *instance = nullptr; - return g_dev; -}; +void start(enum LL40LS_BUS busid); +void stop(); +void test(); +void reset(); +void info(); +void regdump(); +void usage(); /** * Start the driver. */ -void start(const bool use_i2c, const int bus) +void start(enum LL40LS_BUS busid) { - if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) { - errx(1, "driver already started"); - } + int fd, ret; - if (use_i2c) { - /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) { - errx(0, "already started external"); - } - - g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); - - if (g_dev_ext != nullptr && PX4_OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; - - if (bus == PX4_I2C_BUS_EXPANSION) { - goto fail; - } - } - } - -#ifdef PX4_I2C_BUS_ONBOARD - - /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) { - errx(0, "already started internal"); - } - - g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); - - if (g_dev_int != nullptr && PX4_OK != g_dev_int->init()) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + if (instance) { + warnx("driver already started"); + } - if (bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } - } + if (busid == LL40LS_BUS_PWM) { + instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } + if (!instance) { + warnx("Failed to instantiate LidarLitePWM"); + return; } -#endif - - /* set the poll rate to default, starts automatic data collection */ - if (g_dev_int != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - - if (fd == -1) { - goto fail; - } - - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); - - if (ret < 0) { - goto fail; - } + if (instance->init() != PX4_OK) { + warnx("failed to initialize LidarLitePWM"); + goto fail; } - if (g_dev_ext != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - - if (fd == -1) { - goto fail; + } else { + for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) { + if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) { + continue; } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname); - if (ret < 0) { - goto fail; + if (!instance) { + warnx("Failed to instantiate LidarLiteI2C"); + return; } - } - - } else { - g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); - - if (g_dev_pwm != nullptr && PX4_OK != g_dev_pwm->init()) { - delete g_dev_pwm; - g_dev_pwm = nullptr; - warnx("failed to init PWM"); - } - if (g_dev_pwm != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_PWM, O_RDONLY); - - if (fd == -1) { - warnx("fd nothing"); - goto fail; + if (instance->init() == PX4_OK) { + break; } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); - - if (ret < 0) { - warnx("pollrate fail"); - goto fail; - } + warnx("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); + delete instance; + instance = nullptr; } } - exit(0); - -fail: + if (!instance) { + warnx("No LidarLite found"); + return; + } -#ifdef PX4_I2C_BUS_ONBOARD + fd = open(instance->get_dev_name(), O_RDONLY); - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { - delete g_dev_int; - g_dev_int = nullptr; + if (fd == -1) { + warnx("Error opening fd"); + goto fail; } -#endif + ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); - if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { - delete g_dev_ext; - g_dev_ext = nullptr; + if (ret < 0) { + warnx("pollrate fail"); + goto fail; } - if (g_dev_pwm != nullptr) { - delete g_dev_pwm; - g_dev_pwm = nullptr; - } + return; - errx(1, "driver start failed"); +fail: + delete instance; + instance = nullptr; } /** * Stop the driver */ -void stop(const bool use_i2c, const int bus) +void stop() { - if (use_i2c) { - if (bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - } else { - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } - } - - } else { - if (g_dev_pwm != nullptr) { - delete g_dev_pwm; - g_dev_pwm = nullptr; - } - } - - exit(0); + delete instance; + instance = nullptr; } /** @@ -282,32 +190,30 @@ void stop(const bool use_i2c, const int bus) * and automatic modes. */ void -test(const bool use_i2c, const int bus) +test() { struct distance_sensor_s report; ssize_t sz; int ret; - const char *path; - - if (use_i2c) { - path = ((bus == -1 || bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); - - } else { - path = LL40LS_DEVICE_PATH_PWM; + if (!instance) { + warnx("No ll40ls driver running"); + errx(1, "FAIL"); } - int fd = open(path, O_RDONLY); + int fd = open(instance->get_dev_name(), O_RDONLY); if (fd < 0) { - err(1, "%s %s open failed, is the driver running?", (use_i2c) ? "I2C" : "PWM", path); + warnx("Error opening fd"); + errx(1, "FAIL"); } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "immediate read failed"); + warnx("immediate read failed"); + goto error; } warnx("single read"); @@ -316,7 +222,8 @@ test(const bool use_i2c, const int bus) /* start the sensor polling at 2Hz */ if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - errx(1, "failed to set 2Hz poll rate"); + warnx("failed to set 2Hz poll rate"); + goto error; } /* read the sensor 5 times and report each value */ @@ -329,14 +236,16 @@ test(const bool use_i2c, const int bus) ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + warnx("timed out waiting for sensor data"); + goto error; } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + warnx("periodic read failed"); + goto error; } warnx("periodic read %u", i); @@ -348,67 +257,78 @@ test(const bool use_i2c, const int bus) /* reset the sensor polling to default rate */ if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set default poll rate"); + warnx("failed to set default poll rate"); + goto error; } + close(fd); errx(0, "PASS"); + +error: + close(fd); + errx(1, "FAIL"); } /** * Reset the driver. */ void -reset(const bool use_i2c, const int bus) +reset() { - - const char *path; - - if (use_i2c) { - path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); - - } else { - path = LL40LS_DEVICE_PATH_PWM; + if (!instance) { + warnx("No ll40ls driver running"); + return; } - int fd = open(path, O_RDONLY); + int fd = open(instance->get_dev_name(), O_RDONLY); if (fd < 0) { - err(1, "failed "); + warnx("Error opening fd"); + return; } if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); + warnx("driver reset failed"); + goto error; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); + warnx("driver poll restart failed"); + goto error; } - exit(0); +error: + close(fd); } /** * Print a little info about the driver. */ void -info(const bool use_i2c, const int bus) +info() { - LidarLite *g_dev = get_dev(use_i2c, bus); - printf("state @ %p\n", g_dev); - g_dev->print_info(); - exit(0); + if (!instance) { + warnx("No ll40ls driver running"); + return; + } + + printf("state @ %p\n", instance); + instance->print_info(); } /** * Dump registers */ void -regdump(const bool use_i2c, const int bus) +regdump() { - LidarLite *g_dev = get_dev(use_i2c, bus); - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - exit(0); + if (!instance) { + warnx("No ll40ls driver running"); + return; + } + + printf("regdump @ %p\n", instance); + instance->print_registers(); } void @@ -428,44 +348,41 @@ int ll40ls_main(int argc, char *argv[]) { int ch; - int bus = -1; + enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL; while ((ch = getopt(argc, argv, "XI")) != EOF) { switch (ch) { #ifdef PX4_I2C_BUS_ONBOARD case 'I': - bus = PX4_I2C_BUS_ONBOARD; + busid = LL40LS_BUS_I2C_INTERNAL; break; #endif case 'X': - bus = PX4_I2C_BUS_EXPANSION; + busid = LL40LS_BUS_I2C_EXTERNAL; break; default: ll40ls::usage(); - exit(0); + return 0; } } - /* default to I2C if no protocol is given */ - bool use_i2c = true; - /* determine protocol first because it's needed next */ if (argc > optind + 1) { const char *protocol = argv[optind + 1]; if (!strcmp(protocol, "pwm")) { - use_i2c = false; + busid = LL40LS_BUS_PWM;; } else if (!strcmp(protocol, "i2c")) { - use_i2c = true; + // Do nothing } else { warnx("unknown protocol, choose pwm or i2c"); ll40ls::usage(); - exit(0); + return 0; } } @@ -473,43 +390,32 @@ ll40ls_main(int argc, char *argv[]) if (argc > optind) { const char *verb = argv[optind]; - /* Start/load the driver. */ if (!strcmp(verb, "start")) { - ll40ls::start(use_i2c, bus); - } + ll40ls::start(busid); - /* Stop the driver */ - if (!strcmp(verb, "stop")) { - ll40ls::stop(use_i2c, bus); - } + } else if (!strcmp(verb, "stop")) { + ll40ls::stop(); - /* Test the driver/device. */ - else if (!strcmp(verb, "test")) { - ll40ls::test(use_i2c, bus); - } + } else if (!strcmp(verb, "test")) { + ll40ls::test(); - /* Reset the driver. */ - else if (!strcmp(verb, "reset")) { - ll40ls::reset(use_i2c, bus); - } + } else if (!strcmp(verb, "reset")) { + ll40ls::reset(); - /* dump registers */ - else if (!strcmp(verb, "regdump")) { - ll40ls::regdump(use_i2c, bus); - } + } else if (!strcmp(verb, "regdump")) { + ll40ls::regdump(); - /* Print driver information. */ - else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - ll40ls::info(use_i2c, bus); - } + } else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(); - else { + } else { ll40ls::usage(); - exit(0); } + + return 0; } warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); ll40ls::usage(); - exit(0); + return 0; } From 3098c77c9168553c5085636fc4b0428e78568bc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 17 Apr 2017 17:22:14 -0700 Subject: [PATCH 06/29] aerofc: Use the additional I2C This change plus the new FPGA RTL(version 0xC1 or higher) will make use of the new I2C bus, this new bus will be shared between aerofc_adc and ll40ls(if connected) and leaving the old bus just to IST8310. --- src/drivers/boards/aerofc-v1/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/aerofc-v1/board_config.h b/src/drivers/boards/aerofc-v1/board_config.h index 9f76b381eed5..424d8bffdf2f 100644 --- a/src/drivers/boards/aerofc-v1/board_config.h +++ b/src/drivers/boards/aerofc-v1/board_config.h @@ -76,7 +76,8 @@ * */ -#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_EXPANSION1 2 #define PX4_I2C_BUS_ONBOARD 3 #define PX4_I2C_OBDEV_HMC5883 0x1e From a0fe6f2ebe868e62116d22560528b6f1840df804 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 24 Jul 2017 14:15:44 -0700 Subject: [PATCH 07/29] aerofc: Reboot board when force bootloader pin is set This can help "unbrick" AeroFC when a bad firmware is loaded and it keeps rebooting or it spinning in some loop. No need to request to stay in booloader as it will stay in bootloader because the pin is set. --- src/drivers/boards/aerofc-v1/aerofc_init.c | 22 +++++++++++++++++++-- src/drivers/boards/aerofc-v1/board_config.h | 2 ++ 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/aerofc-v1/aerofc_init.c b/src/drivers/boards/aerofc-v1/aerofc_init.c index ed4fa212de1b..55480d5bc2e1 100644 --- a/src/drivers/boards/aerofc-v1/aerofc_init.c +++ b/src/drivers/boards/aerofc-v1/aerofc_init.c @@ -115,6 +115,19 @@ extern void led_on(int led); extern void led_off(int led); __END_DECLS +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int _bootloader_force_pin_callback(int irq, void *context) +{ + if (stm32_gpioread(GPIO_FORCE_BOOTLOADER)) { + board_reset(0); + } + + return 0; +} + /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -128,15 +141,17 @@ __END_DECLS * * Description: * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured + * is called early in the initialization -- after all memory has been configured * and mapped but before any devices have been initialized. * ************************************************************************************/ __EXPORT void stm32_boardinitialize(void) { - /* configure LEDs */ + stm32_configgpio(GPIO_FORCE_BOOTLOADER); + _bootloader_force_pin_callback(0, NULL); + /* configure LEDs */ board_autoled_initialize(); /* turn sensors on */ @@ -159,6 +174,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) { int result; + /* the interruption subsystem is not initialized when stm32_boardinitialize() is called */ + stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback); + #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) /* run C++ ctors before we go any further */ diff --git a/src/drivers/boards/aerofc-v1/board_config.h b/src/drivers/boards/aerofc-v1/board_config.h index 424d8bffdf2f..1d1e958d56bc 100644 --- a/src/drivers/boards/aerofc-v1/board_config.h +++ b/src/drivers/boards/aerofc-v1/board_config.h @@ -64,6 +64,8 @@ #define GPIO_VDD_5V_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN13) +#define GPIO_FORCE_BOOTLOADER (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN11|GPIO_EXTI) + /* * I2C busses * From 00f8024f5848510a48dd62e05b29402faf28afe5 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 28 Aug 2017 10:26:59 -0700 Subject: [PATCH 08/29] aerofc: switch companion baud to 921600 --- ROMFS/px4fmu_common/init.d/4070_aerofc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4070_aerofc b/ROMFS/px4fmu_common/init.d/4070_aerofc index 014a66613d5d..e7ba28e367dc 100644 --- a/ROMFS/px4fmu_common/init.d/4070_aerofc +++ b/ROMFS/px4fmu_common/init.d/4070_aerofc @@ -76,5 +76,5 @@ set OUTPUT_MODE tap_esc set MIXER quad_x set USE_IO no -param set SYS_COMPANION 460800 +param set SYS_COMPANION 921600 set MAVLINK_COMPANION_DEVICE /dev/ttyS1 From d928fd3bb9c7cd264a8587346d49df9ecbee8782 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 28 Aug 2017 10:26:25 -0700 Subject: [PATCH 09/29] aerofc: allow to use 921600 baud to reboot --- Tools/aero_upload.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/aero_upload.sh b/Tools/aero_upload.sh index 9a6777645047..d63e27f3024f 100755 --- a/Tools/aero_upload.sh +++ b/Tools/aero_upload.sh @@ -62,7 +62,7 @@ ssh $target /bin/bash < Date: Fri, 1 Dec 2017 11:04:21 +0100 Subject: [PATCH 10/29] * added new airframe, new mixer and kitepower vtol controller (still based on tailsitter) * added new vtol type * kill switch causes automatically disarming * RC loss causes automatically disarming --- .../init.d/13014_kitepower_aeolus250 | 22 + .../mixers/kitepower_aeolus250.main.mix | 38 ++ posix-configs/SITL/init/ekf2/standard_vtol | 4 +- src/modules/commander/commander.cpp | 33 +- .../systemlib/mixer/mixer_multirotor.cpp | 3 + src/modules/systemlib/mixer/multi_tables.py | 10 +- src/modules/vtol_att_control/CMakeLists.txt | 1 + src/modules/vtol_att_control/kitepower.cpp | 515 ++++++++++++++++++ src/modules/vtol_att_control/kitepower.h | 128 +++++ .../vtol_att_control/kitepower_params.c | 57 ++ .../vtol_att_control_main.cpp | 3 +- .../vtol_att_control/vtol_att_control_main.h | 1 + .../vtol_att_control_params.c | 6 +- src/modules/vtol_att_control/vtol_type.h | 3 +- 14 files changed, 812 insertions(+), 12 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 create mode 100644 ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix create mode 100644 src/modules/vtol_att_control/kitepower.cpp create mode 100644 src/modules/vtol_att_control/kitepower.h create mode 100644 src/modules/vtol_att_control/kitepower_params.c diff --git a/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 b/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 new file mode 100644 index 000000000000..4725af2a984e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 @@ -0,0 +1,22 @@ +#!nsh +# +# @name VTOL Aeolus250 +# @class VTOL +# +# @type VTOL Quad Tailsitter +# +# @output MAIN5 RUDDER +# @output MAIN6 ELEVATOR +# +# @maintainer Gabriel Koenig +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER kitepower_aeolus250 + +set PWM_OUT 1234 +set MAV_TYPE 20 +set VT_TYPE 3 +set PWM_MIN 1100 +set CBRK_USB_CHK 197848 diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix new file mode 100644 index 000000000000..964e42042096 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -0,0 +1,38 @@ +Caipirinha tailsitter mixer +============================ + +This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle +has two motors in total, one attached to each wing. It also has two elevons which +are located in the slipstream of the propellers. This mixer generates 4 PWM outputs +on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the +elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run +at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. + +Motor mixer +------------ +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. + +R: 4aeol 10000 10000 10000 0 + +5. main output: Using Control Group 1 (VTOL) pitch +------------------------------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +6. main output: using Control Group 1 (VTOL) yaw +------------------------------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 10000 10000 0 -10000 10000 + +Z: + +8. main output: Parachute Release +------------------ +Using control group to +M: 2 +O: 10000 10000 0 -10000 10000 +S: 2 7 10000 10000 0 -10000 10000 +S: 3 5 -10000 -10000 0 -10000 10000 diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index 95c9ce261ec8..dc87f7f5fb97 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -52,7 +52,7 @@ param set RTL_RETURN_ALT 30.0 param set SENS_BOARD_ROT 0 param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 -param set SYS_AUTOSTART 13006 +param set SYS_AUTOSTART 13014 param set SYS_MC_EST_GROUP 2 param set SYS_RESTART_TYPE 2 param set VT_MOT_COUNT 4 @@ -78,7 +78,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix +mixer load /dev/pwm_output0 ROMFS/sitl/mixers/kitepower_aeolus250.mix mavlink start -u 14556 -r 2000000 mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc625310d891..2de13423b123 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2676,19 +2676,24 @@ int commander_thread_main(int argc, char *argv[]) _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; - if (in_armed_state && + if ((in_armed_state && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && - (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition) ) { + (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) + || (in_armed_state && (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON)) // Add rule for killswitch + ) { if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && internal_state.main_state != commander_state_s::MAIN_STATE_STAB && - internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && - !land_detector.landed) { + internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)// && + //!land_detector.landed) // Remove landed condition + { print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); - } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { + } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition + || sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON // Add rule for killswitch + ) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -2826,6 +2831,24 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; + // Add disarm code + /* disarm for lockdown */ + //transition_result_t arming_ret; + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); + //arming_ret = + arming_state_transition(&status, + &battery, + &safety, + new_arming_state, + &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, + &status_flags, + avionics_power_rail_voltage, + arm_without_gps, + arm_mission_required, + hrt_elapsed_time(&commander_boot_timestamp)); } } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index ac5afbbb002a..00c2b2bdf0f2 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -136,6 +136,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorGeometry::QUAD_X; + } else if (!strcmp(geomname, "4aeol")){ + geometry = MultirotorGeometry::KITEPOWER_AEOLUS250; + } else if (!strcmp(geomname, "4h")) { geometry = MultirotorGeometry::QUAD_H; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index 6765efdfeae8..313b75c8f35e 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -62,6 +62,14 @@ def rcos(angleInRadians): [135, CW], ] +#just defines motor position, order doesn't matter +kitepower_aeolus250 = [ + [ 34, CCW], + [-146, CCW], + [ -34, CW], + [ 146, CW], +] + quad_h = [ [ 45, CW], [-135, CW], @@ -213,7 +221,7 @@ def rcos(angleInRadians): [-150, CCW], ] -tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_s250aq, quad_deadcat, hex_x, hex_plus, hex_cox, hex_t, octa_x, octa_plus, octa_cox, octa_cox_wide, twin_engine, tri_y, dodeca_top_cox, dodeca_bottom_cox] +tables = [quad_x, kitepower_aeolus250, quad_h, quad_plus, quad_v, quad_wide, quad_s250aq, quad_deadcat, hex_x, hex_plus, hex_cox, hex_t, octa_x, octa_plus, octa_cox, octa_cox_wide, twin_engine, tri_y, dodeca_top_cox, dodeca_bottom_cox] def variableName(variable): for variableName, value in list(globals().items()): diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 17d8dd242e68..4acf9b07c9d2 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( vtol_type.cpp tailsitter.cpp standard.cpp + kitepower.cpp DEPENDS platforms__common ) diff --git a/src/modules/vtol_att_control/kitepower.cpp b/src/modules/vtol_att_control/kitepower.cpp new file mode 100644 index 000000000000..00db39a8ef76 --- /dev/null +++ b/src/modules/vtol_att_control/kitepower.cpp @@ -0,0 +1,515 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file kitepower.cpp +* +* @author Roman Babst +* @author Gabriel König +* +*/ + +#include "kitepower.h" +#include "vtol_att_control_main.h" + +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC + +Kitepower::Kitepower(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _min_front_trans_dur(0.5f), + _thrust_transition_start(0.0f), + _yaw_transition(0.0f), + _pitch_transition_start(0.0f), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-kitepower")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-kitepower nonfinite input")) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_kitepower.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_kitepower.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_kitepower.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_kitepower.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_kitepower.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_kitepower.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + +} + +Kitepower::~Kitepower() +{ + +} + +void +Kitepower::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_kitepower.front_trans_dur, &v); + _params_kitepower + .front_trans_dur = math::constrain(v, 1.0f, 5.0f); + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_kitepower.front_trans_dur_p2, &v); + _params_kitepower.front_trans_dur_p2 = v; + + /* vtol duration of a back transition */ + param_get(_params_handles_kitepower.back_trans_dur, &v); + _params_kitepower.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_kitepower.airspeed_trans, &v); + _params_kitepower.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_kitepower.airspeed_blend_start, &v); + _params_kitepower.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_kitepower.elevons_mc_lock, &l); + _params_kitepower.elevons_mc_lock = l; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_kitepower.front_trans_dur = math::max(_params_kitepower.front_trans_dur, _min_front_trans_dur); + + if (_params_kitepower.airspeed_trans < _params_kitepower.airspeed_blend_start + 1.0f) { + _params_kitepower.airspeed_trans = _params_kitepower.airspeed_blend_start + 1.0f; + } +} + +void Kitepower::update_vtol_state() +{ + + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. + */ + + //matrix::Eulerf euler = matrix::Quatf(_v_att->q); + // Kitepower: not needed at the moment + //float pitch = euler.theta(); + + if (!_attc->is_fixed_wing_requested()) { + + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + //Kitepower: direct switch for the moment for transition + // check if we have reached pitch angle to switch to MC mode + /*if (pitch >= PITCH_TRANSITION_BACK) { + _vtol_schedule.flight_mode = MC_MODE; + }*/ + _vtol_schedule.flight_mode = MC_MODE; + + break; + } + + } else { // user switchig to FW mode + + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + //Kitpower: direct switch for the moment for front transition + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + /*if ((_airspeed->indicated_airspeed_m_s >= _params_kitepower.airspeed_trans + && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + }*/ + _vtol_schedule.flight_mode = FW_MODE; + + break; + + case TRANSITION_FRONT_P2: + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; + } + } + + // map kitepower specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION_TO_FW; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION_TO_FW; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION_TO_MC; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; + } +} + +void Kitepower::update_transition_state() +{ + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att_sp->yaw_body; + _pitch_transition_start = _v_att_sp->pitch_body; + _thrust_transition_start = _v_att_sp->thrust; + _flag_was_in_trans_mode = true; + } + + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f, + _pitch_transition_start); + + /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ + if (_airspeed->indicated_airspeed_m_s <= _params_kitepower.airspeed_trans) { + _thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur * 1000000.0f)); + _thrust_transition = math::constrain(_thrust_transition, _thrust_transition_start, + (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start); + _v_att_sp->thrust = _thrust_transition; + } + + // disable mc yaw control once the plane has picked up speed + if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + + } else { + _mc_yaw_weight = 1.0f; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down + + /** no motor switching */ + + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; + } + + } + + _v_att_sp->thrust = _thrust_transition; + + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ + + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); + //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); + + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f); + + // throttle value is decreesed + _v_att_sp->thrust = _thrust_transition_start * 0.9f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_kitepower.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_kitepower.back_trans_dur * 1000000.0f); + + } + + + + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0.0f; + _v_att_sp->yaw_body = _yaw_transition; + + math::Quaternion q_sp; + q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); +} + +void Kitepower::waiting_on_tecs() +{ + // copy the last trust value from the front transition + _v_att_sp->thrust = _thrust_transition; +} + +void Kitepower::calc_tot_airspeed() +{ + float airspeed = math::max(1.0f, _airspeed->indicated_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; + P = math::constrain(P, 1.0f, _params->power_max); + // calculate prop efficiency + float power_factor = 1.0f - P * _params->prop_eff / _params->power_max; + float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f; + eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed / eta - airspeed) * 2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + +void Kitepower::scale_mc_output() +{ + // scale around tuning airspeed + float airspeed; + calc_tot_airspeed(); // estimate air velocity seen by elevons + + // if airspeed is not updating, we assume the normal average speed + if (bool nonfinite = !PX4_ISFINITE(_airspeed->indicated_airspeed_m_s) || + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + airspeed = _params->mc_airspeed_trim; + + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); + } + + _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : + airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, + -1.0f, 1.0f); +} + +void Kitepower::update_mc_state() +{ + VtolType::update_mc_state(); + + // set idle speed for rotary wing mode + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Kitepower::update_fw_state() +{ + VtolType::update_fw_state(); + + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } +} + +/** +* Write data to actuator output topic. +*/ +void Kitepower::fill_actuator_outputs() +{ + switch (_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; + + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + + break; + + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->timestamp = _actuators_fw_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; + } +} diff --git a/src/modules/vtol_att_control/kitepower.h b/src/modules/vtol_att_control/kitepower.h new file mode 100644 index 000000000000..a7ba9d372e40 --- /dev/null +++ b/src/modules/vtol_att_control/kitepower.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file kitepower.h +* +* @author Roman Bapst +* @author Gabriel König +* +*/ + +#ifndef KITEPOWER_H +#define KITEPOWER_H + +#include "vtol_type.h" +#include /** is it necsacery? **/ +#include +#include + +class Kitepower : public VtolType +{ + +public: + Kitepower(VtolAttitudeControl *_att_controller); + ~Kitepower(); + + virtual void update_vtol_state(); + virtual void update_transition_state(); + virtual void update_mc_state(); + virtual void update_fw_state(); + virtual void fill_actuator_outputs(); + virtual void waiting_on_tecs(); + +private: + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float front_trans_dur_p2; + float back_trans_dur; /**< duration of back transition */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + + } _params_kitepower; + + struct { + param_t front_trans_dur; + param_t front_trans_dur_p2; + param_t back_trans_dur; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + + } _params_handles_kitepower; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ + + /** not sure about it yet ?! **/ + float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + float _thrust_transition_start; // throttle value when we start the front transition + float _yaw_transition; // yaw angle in which transition will take place + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + + + /** should this anouncement stay? **/ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + + /** + * Speed estimation for propwash controlled surfaces. + */ + void calc_tot_airspeed(); + + + /** is this one still needed? */ + void scale_mc_output(); + + /** + * Update parameters. + */ + virtual void parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/kitepower_params.c b/src/modules/vtol_att_control/kitepower_params.c new file mode 100644 index 000000000000..e4cf2a1c326f --- /dev/null +++ b/src/modules/vtol_att_control/kitepower_params.c @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +#include + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @unit s + * @min 0.1 + * @max 5.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 7f19fced95c8..d356ecd4f0cc 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -142,7 +142,8 @@ VtolAttitudeControl::VtolAttitudeControl() : if (_params.vtol_type == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); - + } else if (_params.vtol_type ==vtol_type::KITEPOWER) { + _vtol_type = new Kitepower(this); } else if (_params.vtol_type == vtol_type::TILTROTOR) { _vtol_type = new Tiltrotor(this); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 5862317cce76..450bd438b295 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include "tiltrotor.h" #include "tailsitter.h" #include "standard.h" +#include "kitepower.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index c9e6cffd4907..5e5482e53252 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -170,13 +170,15 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** - * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2, Kitepower=3) * * @value 0 Tailsitter * @value 1 Tiltrotor * @value 2 Standard + * @value 3 Kitepower + * @min 0 - * @max 2 + * @max 3 * @decimal 0 * @group VTOL Attitude Control */ diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index e298ec3a6cd8..9999579bfa7a 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -78,7 +78,8 @@ enum mode { enum vtol_type { TAILSITTER = 0, TILTROTOR, - STANDARD + STANDARD, + KITEPOWER, }; class VtolAttitudeControl; From 8a0169f0b8aa5ce4584d0fdbe5a456a7ff4331da Mon Sep 17 00:00:00 2001 From: gkoenig Date: Wed, 17 Jan 2018 12:48:58 +0100 Subject: [PATCH 11/29] * Add rigid_wing target for gazebo_sitl repo * Add sitl mixer file * reversed standard_vtol mixer back --- .gitmodules | 2 +- .../mixers/kitepower_aeolus250.main.mix | 2 +- .../mixers/kitepower_aeolus250_sitl.main.mix | 37 +++++++ Tools/sitl_gazebo | 2 +- posix-configs/SITL/init/ekf2/rigid_wing | 96 +++++++++++++++++++ posix-configs/SITL/init/ekf2/standard_vtol | 4 +- src/firmware/posix/sitl_target.cmake | 2 +- 7 files changed, 139 insertions(+), 6 deletions(-) create mode 100644 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix create mode 100644 posix-configs/SITL/init/ekf2/rigid_wing diff --git a/.gitmodules b/.gitmodules index 26451efda8d6..dbbfd7cdf5dc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -21,7 +21,7 @@ url = https://github.com/PX4/jMAVSim.git [submodule "Tools/sitl_gazebo"] path = Tools/sitl_gazebo - url = https://github.com/PX4/sitl_gazebo.git + url = git@192.168.11.131:rigidWing/sitl_gazebo.git [submodule "src/lib/matrix"] path = src/lib/matrix url = https://github.com/PX4/Matrix.git diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index 964e42042096..912a118018f1 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -1,4 +1,4 @@ -Caipirinha tailsitter mixer +kitepower aeolus mixer ============================ This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle diff --git a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix new file mode 100644 index 000000000000..3174fa0b6fb9 --- /dev/null +++ b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix @@ -0,0 +1,37 @@ +kitepower aeolus mixer +============================ + +This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle +has two motors in total, one attached to each wing. It also has two elevons which +are located in the slipstream of the propellers. This mixer generates 4 PWM outputs +on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the +elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run +at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. + +Motor mixer +------------ +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. + +R: 4aeol 10000 10000 10000 0 + +5. main output: Using Control Group 1 (VTOL) pitch +------------------------------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +6. main output: using Control Group 1 (VTOL) yaw +------------------------------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 10000 10000 0 -10000 10000 + +Z: + +8. main output: Parachute Release +------------------ +Using control group to +M: 2 +O: 10000 10000 0 -10000 10000 +S: 2 7 10000 10000 0 -10000 10000 diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 17afb5735b95..06768e1ea10b 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 17afb5735b95a9396494f960687ffb28e5dc7d11 +Subproject commit 06768e1ea10b1692ded961096f584f37b000492f diff --git a/posix-configs/SITL/init/ekf2/rigid_wing b/posix-configs/SITL/init/ekf2/rigid_wing new file mode 100644 index 000000000000..c2dbf36b92f1 --- /dev/null +++ b/posix-configs/SITL/init/ekf2/rigid_wing @@ -0,0 +1,96 @@ +uorb start +param load +dataman start +param set BAT_N_CELLS 3 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_ID 1310728 +param set CAL_ACC1_XOFF 0.01 +param set CAL_GYRO0_ID 2293768 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_MAG0_ID 196616 +param set CAL_MAG0_XOFF 0.01 +param set COM_DISARM_LAND 5 +param set COM_RC_IN_MODE 1 +param set EKF2_AID_MASK 1 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_HGT_MODE 0 +param set EKF2_MAG_TYPE 1 +param set FW_AIRSPD_MAX 25 +param set FW_AIRSPD_MIN 14 +param set FW_AIRSPD_TRIM 16 +param set MAV_TYPE 22 +param set MC_PITCH_P 6 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.3 +param set MIS_LTRMIN_ALT 10 +param set MIS_TAKEOFF_ALT 10 +param set MIS_YAW_TMT 10 +param set MPC_ACC_HOR_MAX 2 +param set MPC_ACC_HOR_MAX 2.0 +param set MPC_TKO_SPEED 1.0 +param set MPC_XY_P 0.8 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_VEL_I 0.2 +param set MPC_XY_VEL_P 0.15 +param set MPC_Z_VEL_I 0.15 +param set MPC_Z_VEL_MAX_DN 1.5 +param set MPC_Z_VEL_P 0.6 +param set NAV_ACC_RAD 5.0 +param set NAV_DLL_ACT 2 +param set NAV_LOITER_RAD 80 +param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set RTL_RETURN_ALT 30.0 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set SENS_DPRES_OFF 0.001 +param set SYS_AUTOSTART 13014 +param set SYS_MC_EST_GROUP 2 +param set SYS_RESTART_TYPE 2 +param set VT_MOT_COUNT 4 +param set VT_TRANS_THR 0.75 +param set VT_TYPE 3 +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 +mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index dc87f7f5fb97..95c9ce261ec8 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -52,7 +52,7 @@ param set RTL_RETURN_ALT 30.0 param set SENS_BOARD_ROT 0 param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 -param set SYS_AUTOSTART 13014 +param set SYS_AUTOSTART 13006 param set SYS_MC_EST_GROUP 2 param set SYS_RESTART_TYPE 2 param set VT_MOT_COUNT 4 @@ -78,7 +78,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ROMFS/sitl/mixers/kitepower_aeolus250.mix +mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix mavlink start -u 14556 -r 2000000 mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 diff --git a/src/firmware/posix/sitl_target.cmake b/src/firmware/posix/sitl_target.cmake index 2e5bd0ae5b65..4a7052b00278 100644 --- a/src/firmware/posix/sitl_target.cmake +++ b/src/firmware/posix/sitl_target.cmake @@ -80,7 +80,7 @@ ExternalProject_Add_Step(sitl_gazebo forceconfigure # create targets for each viewer/model/debugger combination set(viewers none jmavsim gazebo replay) set(debuggers none ide gdb lldb ddd valgrind callgrind) -set(models none iris iris_opt_flow iris_rplidar standard_vtol plane solo tailsitter typhoon_h480 rover) +set(models none iris iris_opt_flow iris_rplidar standard_vtol plane solo tailsitter typhoon_h480 rover rigid_wing) set(all_posix_vmd_make_targets) foreach(viewer ${viewers}) foreach(debugger ${debuggers}) From 48fd8c87463acadafb7e0f19f846f769ffe5c930 Mon Sep 17 00:00:00 2001 From: gkoenig Date: Thu, 18 Jan 2018 12:33:12 +0100 Subject: [PATCH 12/29] Working new kitepower controller within simulation (simple transition, proper outputs) * cleaned up and simplyfied kitpower vtol controller * added neutral axes rotation when switching in fw mode * added motor shutdown when in fixedwing mode --- .../fw_att_control/fw_att_control_main.cpp | 4 +- .../FixedwingPositionControl.cpp | 4 +- src/modules/vtol_att_control/kitepower.cpp | 364 ++++++------------ src/modules/vtol_att_control/kitepower.h | 26 +- .../vtol_att_control/kitepower_params.c | 20 +- src/modules/vtol_att_control/vtol_type.h | 6 +- 6 files changed, 133 insertions(+), 291 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f96a18d47934..d4a36cc46343 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -210,7 +210,7 @@ class FixedwingAttitudeControl float rattitude_thres; - int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor, 2 = standard, 3 = kitepower */ int32_t bat_scale_en; /**< Battery scaling enabled */ @@ -814,7 +814,7 @@ FixedwingAttitudeControl::task_main() _pitch = euler_angles(1); _yaw = euler_angles(2); - if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { + if (_vehicle_status.is_vtol && (_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER)) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1412a9afd061..dc516cc22735 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -631,7 +631,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if ((_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER) && _vehicle_status.is_vtol) { float tmp = accel_body(0); accel_body(0) = -accel_body(2); accel_body(2) = tmp; @@ -1782,7 +1782,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if ((_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER) && _vehicle_status.is_vtol) { math::Matrix<3, 3> R_offset; R_offset.from_euler(0, M_PI_2_F, 0); math::Matrix<3, 3> R_fixed_wing = _R_nb * R_offset; diff --git a/src/modules/vtol_att_control/kitepower.cpp b/src/modules/vtol_att_control/kitepower.cpp index 00db39a8ef76..00e775c58bdd 100644 --- a/src/modules/vtol_att_control/kitepower.cpp +++ b/src/modules/vtol_att_control/kitepower.cpp @@ -35,23 +35,18 @@ * @file kitepower.cpp * * @author Roman Babst -* @author Gabriel König +* @author Gabriel Koenig * */ #include "kitepower.h" #include "vtol_att_control_main.h" -#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition -#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition -#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 -#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW -#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC Kitepower::Kitepower(VtolAttitudeControl *attc) : VtolType(attc), _airspeed_tot(0.0f), - _min_front_trans_dur(0.5f), + _flag_enable_mc_motors(true), _thrust_transition_start(0.0f), _yaw_transition(0.0f), _pitch_transition_start(0.0f), @@ -64,15 +59,10 @@ Kitepower::Kitepower(VtolAttitudeControl *attc) : _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; _flag_was_in_trans_mode = false; - _params_handles_kitepower.front_trans_dur = param_find("VT_F_TRANS_DUR"); - _params_handles_kitepower.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_kitepower.back_trans_dur = param_find("VT_B_TRANS_DUR"); - _params_handles_kitepower.airspeed_trans = param_find("VT_ARSP_TRANS"); - _params_handles_kitepower.airspeed_blend_start = param_find("VT_ARSP_BLEND"); - _params_handles_kitepower.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); } @@ -84,40 +74,7 @@ Kitepower::~Kitepower() void Kitepower::parameters_update() { - float v; - int l; - /* vtol duration of a front transition */ - param_get(_params_handles_kitepower.front_trans_dur, &v); - _params_kitepower - .front_trans_dur = math::constrain(v, 1.0f, 5.0f); - - /* vtol front transition phase 2 duration */ - param_get(_params_handles_kitepower.front_trans_dur_p2, &v); - _params_kitepower.front_trans_dur_p2 = v; - - /* vtol duration of a back transition */ - param_get(_params_handles_kitepower.back_trans_dur, &v); - _params_kitepower.back_trans_dur = math::constrain(v, 0.0f, 5.0f); - - /* vtol airspeed at which it is ok to switch to fw mode */ - param_get(_params_handles_kitepower.airspeed_trans, &v); - _params_kitepower.airspeed_trans = v; - - /* vtol airspeed at which we start blending mc/fw controls */ - param_get(_params_handles_kitepower.airspeed_blend_start, &v); - _params_kitepower.airspeed_blend_start = v; - - /* vtol lock elevons in multicopter */ - param_get(_params_handles_kitepower.elevons_mc_lock, &l); - _params_kitepower.elevons_mc_lock = l; - - /* avoid parameters which will lead to zero division in the transition code */ - _params_kitepower.front_trans_dur = math::max(_params_kitepower.front_trans_dur, _min_front_trans_dur); - - if (_params_kitepower.airspeed_trans < _params_kitepower.airspeed_blend_start + 1.0f) { - _params_kitepower.airspeed_trans = _params_kitepower.airspeed_blend_start + 1.0f; - } } void Kitepower::update_vtol_state() @@ -143,68 +100,56 @@ void Kitepower::update_vtol_state() case FW_MODE: _vtol_schedule.flight_mode = TRANSITION_BACK; _vtol_schedule.transition_start = hrt_absolute_time(); - break; + _flag_enable_mc_motors = true; + break; - case TRANSITION_FRONT_P1: + case TRANSITION_FRONT: // failsafe into multicopter mode _vtol_schedule.flight_mode = MC_MODE; + _flag_enable_mc_motors = true; break; - case TRANSITION_FRONT_P2: - // NOT USED - // failsafe into multicopter mode - //_vtol_schedule.flight_mode = MC_MODE; - break; case TRANSITION_BACK: - //Kitepower: direct switch for the moment for transition - // check if we have reached pitch angle to switch to MC mode - /*if (pitch >= PITCH_TRANSITION_BACK) { - _vtol_schedule.flight_mode = MC_MODE; - }*/ - _vtol_schedule.flight_mode = MC_MODE; - - break; + //Kitepower: direct switch for the moment for transition + _vtol_schedule.flight_mode = MC_MODE; + _flag_enable_mc_motors = true; + break; } } else { // user switchig to FW mode switch (_vtol_schedule.flight_mode) { case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT; _vtol_schedule.transition_start = hrt_absolute_time(); + _flag_enable_mc_motors = false; break; case FW_MODE: - break; - - case TRANSITION_FRONT_P1: - //Kitpower: direct switch for the moment for front transition - // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - /*if ((_airspeed->indicated_airspeed_m_s >= _params_kitepower.airspeed_trans - && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) { - _vtol_schedule.flight_mode = FW_MODE; - //_vtol_schedule.transition_start = hrt_absolute_time(); - }*/ - _vtol_schedule.flight_mode = FW_MODE; + _flag_enable_mc_motors = false; + break; + case TRANSITION_FRONT: + //Kitpower: direct switch for the moment for front transition + _vtol_schedule.flight_mode = FW_MODE; + _flag_enable_mc_motors = false; break; - case TRANSITION_FRONT_P2: - case TRANSITION_BACK: // failsafe into fixed wing mode _vtol_schedule.flight_mode = FW_MODE; + _flag_enable_mc_motors = false; - /* **LATER*** if pitch is closer to mc (-45>) - * go to transition P1 - */ - break; + break; } } - + update_external_VTOL_state(); +} +void Kitepower::update_external_VTOL_state() +{ // map kitepower specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { case MC_MODE: @@ -219,12 +164,7 @@ void Kitepower::update_vtol_state() _flag_was_in_trans_mode = false; break; - case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION_TO_FW; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; - - case TRANSITION_FRONT_P2: + case TRANSITION_FRONT: _vtol_mode = TRANSITION_TO_FW; _vtol_vehicle_status->vtol_in_trans_mode = true; break; @@ -246,109 +186,34 @@ void Kitepower::update_transition_state() _flag_was_in_trans_mode = true; } - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { - - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur * 1000000.0f)); - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f, - _pitch_transition_start); - - /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ - if (_airspeed->indicated_airspeed_m_s <= _params_kitepower.airspeed_trans) { - _thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur * 1000000.0f)); - _thrust_transition = math::constrain(_thrust_transition, _thrust_transition_start, - (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start); - _v_att_sp->thrust = _thrust_transition; - } - - // disable mc yaw control once the plane has picked up speed - if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { - _mc_yaw_weight = 0.0f; - - } else { - _mc_yaw_weight = 1.0f; - } - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down - - /** no motor switching */ - - if (flag_idle_mc) { - set_idle_fw(); - flag_idle_mc = false; - } - - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( - &_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); + if (_vtol_schedule.flight_mode == TRANSITION_FRONT) { + // Kitepower: at the moment empty as we have direct switch to fixed wing + // create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; - } - - } - - _v_att_sp->thrust = _thrust_transition; - - /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - - //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); - //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.front_trans_dur_p2 * 1000000.0f)); - - - _mc_roll_weight = 0.0f; - _mc_pitch_weight = 0.0f; - - /** keep yaw disabled */ - _mc_yaw_weight = 0.0f; + // create time dependant throttle signal higher than in MC and growing untillspeed reached */ + // eventually disable mc yaw control once the plane has picked up speed } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; - } - - /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_kitepower.back_trans_dur * 1000000.0f); - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f); + // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ // throttle value is decreesed - _v_att_sp->thrust = _thrust_transition_start * 0.9f; - - /** keep yaw disabled */ - _mc_yaw_weight = 0.0f; - /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / - (_params_kitepower.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / - (_params_kitepower.back_trans_dur * 1000000.0f); + // smoothly move control weight to MC */ } - - _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); - _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); - _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); - // compute desired attitude and thrust setpoint for the transition + // Kitepower: at the moment we just use the same values at the beginning of the transition _v_att_sp->timestamp = hrt_absolute_time(); _v_att_sp->roll_body = 0.0f; - _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->pitch_body = _pitch_transition_start; + _v_att_sp->thrust = _thrust_transition_start; math::Quaternion q_sp; q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); @@ -415,101 +280,94 @@ void Kitepower::scale_mc_output() void Kitepower::update_mc_state() { - VtolType::update_mc_state(); + VtolType::update_mc_state(); + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; + + if (_flag_enable_mc_motors) { + set_max_mc(1940); + _flag_enable_mc_motors = false; + } - // set idle speed for rotary wing mode - if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; - } } void Kitepower::update_fw_state() { - VtolType::update_fw_state(); + VtolType::update_fw_state(); + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; + _mc_throttle_weight = 0.0f; + + if (!_flag_enable_mc_motors) { + set_max_mc(940); + _flag_enable_mc_motors = true; + } + - if (flag_idle_mc) { - set_idle_fw(); - flag_idle_mc = false; - } } /** * Write data to actuator output topic. */ + void Kitepower::fill_actuator_outputs() { - switch (_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->timestamp = _actuators_mc_in->timestamp; - - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - - } else { - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon - } + //Kitepower: fail saife mode needs to be added -> probably later on dronecore side + //fill multicopter controls, index 0 + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]*_mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]*_mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]*_mc_yaw_weight; //needs to be tested + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]*_mc_throttle_weight; + + //fill in fixed wing controls, important actuator for + //in the fw_att_controller there is a coordinate frame transformation while switching to fw mode (neutral position is switched 90° around pitch axis) + //Thus we have a coordinate frame for mc and one + //for fw. Thus mc_yaw!=fw_yaw. + _actuators_out_1->timestamp = _actuators_fw_in->timestamp; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]*(1-_mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]*(1-_mc_yaw_weight); //yaw axis in multicopter mode means roll axis in fixedwing, thus no roll control - break; +} - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->timestamp = _actuators_fw_in->timestamp; - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; +/** +* Disable all multirotor motors when in fw mode. +*/ +void +Kitepower::set_max_mc(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); - case TRANSITION_TO_FW: - case TRANSITION_TO_MC: - // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; - _actuators_out_1->timestamp = _actuators_mc_in->timestamp; - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] - * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * - _mc_yaw_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] - * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - break; + if (fd < 0) { + PX4_WARN("can't open %s", dev); + } - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; - } + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) { + PX4_WARN("failed setting max values"); + } + + px4_close(fd); } diff --git a/src/modules/vtol_att_control/kitepower.h b/src/modules/vtol_att_control/kitepower.h index a7ba9d372e40..d3729e1f2e0d 100644 --- a/src/modules/vtol_att_control/kitepower.h +++ b/src/modules/vtol_att_control/kitepower.h @@ -64,29 +64,20 @@ class Kitepower : public VtolType private: struct { - float front_trans_dur; /**< duration of first part of front transition */ - float front_trans_dur_p2; - float back_trans_dur; /**< duration of back transition */ float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ - int elevons_mc_lock; /**< lock elevons in multicopter mode */ } _params_kitepower; struct { - param_t front_trans_dur; - param_t front_trans_dur_p2; - param_t back_trans_dur; param_t airspeed_trans; param_t airspeed_blend_start; - param_t elevons_mc_lock; } _params_handles_kitepower; enum vtol_mode { - MC_MODE = 0, /**< vtol is in multicopter mode */ - TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ - TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT, /**< vtol is in front transition part 1 mode */ TRANSITION_BACK, /**< vtol is in back transition mode */ FW_MODE /**< vtol is in fixed wing mode */ }; @@ -98,8 +89,8 @@ class Kitepower : public VtolType float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ - /** not sure about it yet ?! **/ - float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + bool _flag_enable_mc_motors; + float _thrust_transition_start; // throttle value when we start the front transition float _yaw_transition; // yaw angle in which transition will take place @@ -122,7 +113,14 @@ class Kitepower : public VtolType /** * Update parameters. */ - virtual void parameters_update(); + virtual void parameters_update(); + + void set_max_mc(unsigned pwm_value); + + /** + * Update external VTOL state + */ + void update_external_VTOL_state(); }; #endif diff --git a/src/modules/vtol_att_control/kitepower_params.c b/src/modules/vtol_att_control/kitepower_params.c index e4cf2a1c326f..2f2a0138082a 100644 --- a/src/modules/vtol_att_control/kitepower_params.c +++ b/src/modules/vtol_att_control/kitepower_params.c @@ -32,26 +32,12 @@ ****************************************************************************/ /** - * @file tailsitter_params.c + * @file kitepower_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst - * @author David Vorsin + * @author Gabriel König */ #include -/** - * Duration of front transition phase 2 - * - * Time in seconds it should take for the rotors to rotate forward completely from the point - * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. - * - * @unit s - * @min 0.1 - * @max 5.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - -PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ +//empty at the moment diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 9999579bfa7a..b15ffc9c3063 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -77,9 +77,9 @@ enum mode { enum vtol_type { TAILSITTER = 0, - TILTROTOR, - STANDARD, - KITEPOWER, + TILTROTOR, + STANDARD, + KITEPOWER }; class VtolAttitudeControl; From 6260b455582f0fc811bf4d9cc3d7d447e710c3d2 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 00:36:59 +0100 Subject: [PATCH 13/29] Update model. --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 06768e1ea10b..3a1a8efe225e 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 06768e1ea10b1692ded961096f584f37b000492f +Subproject commit 3a1a8efe225e57663c78fadd64c51d29dbb76fa7 From 2889e8795cecb4f7840d1cd1841800e62835e007 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 13:00:19 +0100 Subject: [PATCH 14/29] Update submodule --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 3a1a8efe225e..2549e5f6361a 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 3a1a8efe225e57663c78fadd64c51d29dbb76fa7 +Subproject commit 2549e5f6361aa8712afa2cb7fe17ba0282bd2c4c From 8fdc4df7a6c79261455f2b9a45bcd1dc80de49a2 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 13:27:12 +0100 Subject: [PATCH 15/29] submodule update --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 2549e5f6361a..ac113a513e60 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 2549e5f6361aa8712afa2cb7fe17ba0282bd2c4c +Subproject commit ac113a513e60004d43777cf395282f9e7743b35f From 0fac808367fc7d4665da618e0ab67be3078efb82 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 15:14:50 +0100 Subject: [PATCH 16/29] Mixer for yaw and pitch working correctly in simulation. --- .../mixers/kitepower_aeolus250_sitl.main.mix | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix index 3174fa0b6fb9..2deffc0da232 100644 --- a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +++ b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix @@ -10,24 +10,27 @@ at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not use Motor mixer ------------ -Channel 1 connects to the right (starboard) motor. -Channel 2 connects to the left (port) motor. +Channels 0-3 connects to the 4 motors R: 4aeol 10000 10000 10000 0 -5. main output: Using Control Group 1 (VTOL) pitch +Channel 4 is null +Z: + +5. main output: using Control Group 1 (VTOL) yaw -> rudder ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 + +Channel 6 is null +Z: -6. main output: using Control Group 1 (VTOL) yaw +7. main output: Using Control Group 1 (VTOL) pitch -> elevator ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 2 10000 10000 0 -10000 10000 - -Z: +S: 1 1 10000 10000 0 -10000 10000 8. main output: Parachute Release ------------------ From 981b9fc268c3c241ed0944c3710174d02455bfda Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 15:19:05 +0100 Subject: [PATCH 17/29] More descriptive comments in mixed file, and add null channel. --- ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index 912a118018f1..b37c2e8f673e 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -10,11 +10,12 @@ at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not use Motor mixer ------------ -Channel 1 connects to the right (starboard) motor. -Channel 2 connects to the left (port) motor. - R: 4aeol 10000 10000 10000 0 +4. null channel +-------------- +Z. + 5. main output: Using Control Group 1 (VTOL) pitch ------------------------------------------ M: 1 @@ -27,6 +28,8 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 1 2 10000 10000 0 -10000 10000 +7. null channel +-------------- Z: 8. main output: Parachute Release From 7ab2ec632a6c941c532b8881a8a025e9c5457b7a Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 16:34:47 +0100 Subject: [PATCH 18/29] Remove redundant mixer (was only for simulation). Invert yaw channel. --- .../mixers/kitepower_aeolus250.main.mix | 4 +- .../mixers/kitepower_aeolus250_sitl.main.mix | 40 ------------------- posix-configs/SITL/init/ekf2/rigid_wing | 2 +- 3 files changed, 4 insertions(+), 42 deletions(-) delete mode 100644 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index b37c2e8f673e..8c0a097a5cc2 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -10,6 +10,8 @@ at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not use Motor mixer ------------ +Channels 0-3 connects to the 4 motors + R: 4aeol 10000 10000 10000 0 4. null channel @@ -26,7 +28,7 @@ S: 1 1 10000 10000 0 -10000 10000 ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 2 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 7. null channel -------------- diff --git a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix deleted file mode 100644 index 2deffc0da232..000000000000 --- a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +++ /dev/null @@ -1,40 +0,0 @@ -kitepower aeolus mixer -============================ - -This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle -has two motors in total, one attached to each wing. It also has two elevons which -are located in the slipstream of the propellers. This mixer generates 4 PWM outputs -on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the -elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run -at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. - -Motor mixer ------------- -Channels 0-3 connects to the 4 motors - -R: 4aeol 10000 10000 10000 0 - -Channel 4 is null -Z: - -5. main output: using Control Group 1 (VTOL) yaw -> rudder ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 - -Channel 6 is null -Z: - -7. main output: Using Control Group 1 (VTOL) pitch -> elevator ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 - -8. main output: Parachute Release ------------------- -Using control group to -M: 2 -O: 10000 10000 0 -10000 10000 -S: 2 7 10000 10000 0 -10000 10000 diff --git a/posix-configs/SITL/init/ekf2/rigid_wing b/posix-configs/SITL/init/ekf2/rigid_wing index c2dbf36b92f1..445e1705edf4 100644 --- a/posix-configs/SITL/init/ekf2/rigid_wing +++ b/posix-configs/SITL/init/ekf2/rigid_wing @@ -78,7 +78,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix mavlink start -u 14556 -r 2000000 mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 From 8bacbf00afd39205aca66ccb0db4c2a5c17a8d68 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 16:46:31 +0100 Subject: [PATCH 19/29] Different channels in mixers, require a model update. --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index ac113a513e60..817ae8c820ba 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit ac113a513e60004d43777cf395282f9e7743b35f +Subproject commit 817ae8c820bafc7609a8053cd08fe44738df8ff0 From 1a1b67dd3050423ed000bb43e84b105a8fbd65c0 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 18:22:04 +0100 Subject: [PATCH 20/29] Merge mixer files. Remove mixer only for sitl. --- .../mixers/kitepower_aeolus250.main.mix | 23 +++++------ .../mixers/kitepower_aeolus250_sitl.main.mix | 40 ------------------- posix-configs/SITL/init/ekf2/rigid_wing | 2 +- 3 files changed, 12 insertions(+), 53 deletions(-) delete mode 100644 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index b37c2e8f673e..2deffc0da232 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -10,27 +10,27 @@ at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not use Motor mixer ------------ +Channels 0-3 connects to the 4 motors + R: 4aeol 10000 10000 10000 0 -4. null channel --------------- -Z. +Channel 4 is null +Z: -5. main output: Using Control Group 1 (VTOL) pitch +5. main output: using Control Group 1 (VTOL) yaw -> rudder ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 + +Channel 6 is null +Z: -6. main output: using Control Group 1 (VTOL) yaw +7. main output: Using Control Group 1 (VTOL) pitch -> elevator ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 2 10000 10000 0 -10000 10000 - -7. null channel --------------- -Z: +S: 1 1 10000 10000 0 -10000 10000 8. main output: Parachute Release ------------------ @@ -38,4 +38,3 @@ Using control group to M: 2 O: 10000 10000 0 -10000 10000 S: 2 7 10000 10000 0 -10000 10000 -S: 3 5 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix b/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix deleted file mode 100644 index 2deffc0da232..000000000000 --- a/ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +++ /dev/null @@ -1,40 +0,0 @@ -kitepower aeolus mixer -============================ - -This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle -has two motors in total, one attached to each wing. It also has two elevons which -are located in the slipstream of the propellers. This mixer generates 4 PWM outputs -on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the -elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run -at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. - -Motor mixer ------------- -Channels 0-3 connects to the 4 motors - -R: 4aeol 10000 10000 10000 0 - -Channel 4 is null -Z: - -5. main output: using Control Group 1 (VTOL) yaw -> rudder ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 - -Channel 6 is null -Z: - -7. main output: Using Control Group 1 (VTOL) pitch -> elevator ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 - -8. main output: Parachute Release ------------------- -Using control group to -M: 2 -O: 10000 10000 0 -10000 10000 -S: 2 7 10000 10000 0 -10000 10000 diff --git a/posix-configs/SITL/init/ekf2/rigid_wing b/posix-configs/SITL/init/ekf2/rigid_wing index c2dbf36b92f1..445e1705edf4 100644 --- a/posix-configs/SITL/init/ekf2/rigid_wing +++ b/posix-configs/SITL/init/ekf2/rigid_wing @@ -78,7 +78,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ROMFS/sitl/mixers/kitepower_aeolus250_sitl.main.mix +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix mavlink start -u 14556 -r 2000000 mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 From 99f89efd56cc9c74e689cb8e30b03accc61704a3 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 18:31:47 +0100 Subject: [PATCH 21/29] Change channels of yaw and pitch. --- .../mixers/kitepower_aeolus250.main.mix | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index 2deffc0da232..7e2419022e86 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -17,20 +17,20 @@ R: 4aeol 10000 10000 10000 0 Channel 4 is null Z: -5. main output: using Control Group 1 (VTOL) yaw -> rudder +5. main output: Using Control Group 1 (VTOL) pitch -> elevator ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 - -Channel 6 is null -Z: +S: 1 1 10000 10000 0 -10000 10000 -7. main output: Using Control Group 1 (VTOL) pitch -> elevator +6. main output: using Control Group 1 (VTOL) yaw -> rudder ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 + +Channel 7 is null +Z: 8. main output: Parachute Release ------------------ From bb5b88c4ead63a92aef0cd363e9a5b2b29694204 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sat, 20 Jan 2018 19:08:21 +0100 Subject: [PATCH 22/29] submodule update. --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 817ae8c820ba..4606a7b48b86 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 817ae8c820bafc7609a8053cd08fe44738df8ff0 +Subproject commit 4606a7b48b86165ad653e862ac998557c270591c From b2e85f120cb800727b0815dd784a10c8b4c63a7d Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Sun, 18 Feb 2018 15:30:23 +0100 Subject: [PATCH 23/29] Use control group target from mavlink message. --- src/modules/mavlink/mavlink_receiver.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 74d6c5ca5b95..7022674e014a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1055,17 +1055,27 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m if (_control_mode.flag_control_offboard_enabled) { actuator_controls.timestamp = hrt_absolute_time(); + uint8_t control_group = set_actuator_control_target.group_mlx; /* Set duty cycles for the servos in actuator_controls_0 */ - for (size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = set_actuator_control_target.controls[i]; - } + memcpy(actuator_controls.control, set_actuator_control_target.controls, 8*sizeof(float)); if (_actuator_controls_pub == nullptr) { - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - + switch(control_group){ + case 0: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); break; + case 1: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls); break; + case 2: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls); break; + case 3: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls); break; + default: break; + } } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + switch(control_group){ + case 0: orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); break; + case 1: orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pub, &actuator_controls); break; + case 2: orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pub, &actuator_controls); break; + case 3: orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pub, &actuator_controls); break; + default: break; + } } } } From 05d5acde96c2fe4810a8d5417cd13be91c92b287 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Mon, 19 Feb 2018 09:24:37 +0100 Subject: [PATCH 24/29] Fix style. --- src/modules/mavlink/mavlink_receiver.cpp | 35 +++++++++++++++--------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7022674e014a..d222f2742609 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1058,23 +1058,32 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m uint8_t control_group = set_actuator_control_target.group_mlx; /* Set duty cycles for the servos in actuator_controls_0 */ - memcpy(actuator_controls.control, set_actuator_control_target.controls, 8*sizeof(float)); + memcpy(actuator_controls.control, set_actuator_control_target.controls, 8 * sizeof(float)); if (_actuator_controls_pub == nullptr) { - switch(control_group){ - case 0: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); break; - case 1: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls); break; - case 2: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls); break; - case 3: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls); break; - default: break; + switch (control_group) { + case 0: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); break; + + case 1: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls); break; + + case 2: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls); break; + + case 3: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls); break; + + default: break; } + } else { - switch(control_group){ - case 0: orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); break; - case 1: orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pub, &actuator_controls); break; - case 2: orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pub, &actuator_controls); break; - case 3: orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pub, &actuator_controls); break; - default: break; + switch (control_group) { + case 0: orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); break; + + case 1: orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pub, &actuator_controls); break; + + case 2: orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pub, &actuator_controls); break; + + case 3: orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pub, &actuator_controls); break; + + default: break; } } } From cd9f33313b9a08dc89517acadadcec7d6dd4cba4 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Tue, 6 Mar 2018 22:30:08 +0100 Subject: [PATCH 25/29] Add check and fix for changing control groups. --- src/modules/mavlink/mavlink_receiver.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d222f2742609..e638d33ddbd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1056,10 +1056,15 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); uint8_t control_group = set_actuator_control_target.group_mlx; - + static int8_t control_group_previous = -1; /* Set duty cycles for the servos in actuator_controls_0 */ memcpy(actuator_controls.control, set_actuator_control_target.controls, 8 * sizeof(float)); + /* Reset orb advertising when current control group is unequal to the previous control group */ + if (!(control_group == control_group_previous)) { + _actuator_controls_pub = nullptr; + } + if (_actuator_controls_pub == nullptr) { switch (control_group) { case 0: _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); break; @@ -1086,6 +1091,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m default: break; } } + + control_group_previous = (int8_t) control_group; } } From e303bae86b1b95f8e98c72687dd681a2803b10f7 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Mon, 26 Mar 2018 23:19:35 +0200 Subject: [PATCH 26/29] Changed !(x==y) to (x!=y). Unadvertise when a different control group is published. --- src/modules/mavlink/mavlink_receiver.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e638d33ddbd2..6671b2c53b9e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1057,11 +1057,14 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); uint8_t control_group = set_actuator_control_target.group_mlx; static int8_t control_group_previous = -1; - /* Set duty cycles for the servos in actuator_controls_0 */ + memcpy(actuator_controls.control, set_actuator_control_target.controls, 8 * sizeof(float)); /* Reset orb advertising when current control group is unequal to the previous control group */ - if (!(control_group == control_group_previous)) { + if (control_group != control_group_previous) { + + orb_unadvertise(_actuator_controls_pub); + _actuator_controls_pub = nullptr; } From 09e5fb5207a5ca1d227d7c1a7e21c09976106909 Mon Sep 17 00:00:00 2001 From: gkoenig Date: Tue, 10 Apr 2018 12:19:08 +0200 Subject: [PATCH 27/29] cleaned up, working servo outputs, proper switch mc to fw --- .../mixers/kitepower_aeolus250.main.mix | 13 ++++----- Tools/sitl_gazebo | 2 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/vtol_att_control/kitepower.cpp | 28 +++++++++++-------- 4 files changed, 24 insertions(+), 21 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index 7e2419022e86..9bbc8eb4d350 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -10,24 +10,22 @@ at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not use Motor mixer ------------ -Channels 0-3 connects to the 4 motors +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. R: 4aeol 10000 10000 10000 0 -Channel 4 is null -Z: - -5. main output: Using Control Group 1 (VTOL) pitch -> elevator +5. main output: Using Control Group 1 (VTOL) pitch ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 S: 1 1 10000 10000 0 -10000 10000 -6. main output: using Control Group 1 (VTOL) yaw -> rudder +6. main output: using Control Group 1 (VTOL) yaw ------------------------------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 +S: 1 2 10000 10000 0 -10000 10000 Channel 7 is null Z: @@ -38,3 +36,4 @@ Using control group to M: 2 O: 10000 10000 0 -10000 10000 S: 2 7 10000 10000 0 -10000 10000 +S: 3 5 -10000 -10000 0 -10000 10000 diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 4606a7b48b86..4271be39545d 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 4606a7b48b86165ad653e862ac998557c270591c +Subproject commit 4271be39545db780340a1bf361ca137ff34519dc diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d4a36cc46343..bd49cbb82333 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -210,7 +210,7 @@ class FixedwingAttitudeControl float rattitude_thres; - int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor, 2 = standard, 3 = kitepower */ + int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ int32_t bat_scale_en; /**< Battery scaling enabled */ diff --git a/src/modules/vtol_att_control/kitepower.cpp b/src/modules/vtol_att_control/kitepower.cpp index 00e775c58bdd..fa67f25f9e96 100644 --- a/src/modules/vtol_att_control/kitepower.cpp +++ b/src/modules/vtol_att_control/kitepower.cpp @@ -100,13 +100,13 @@ void Kitepower::update_vtol_state() case FW_MODE: _vtol_schedule.flight_mode = TRANSITION_BACK; _vtol_schedule.transition_start = hrt_absolute_time(); - _flag_enable_mc_motors = true; + //_flag_enable_mc_motors = true; break; case TRANSITION_FRONT: // failsafe into multicopter mode _vtol_schedule.flight_mode = MC_MODE; - _flag_enable_mc_motors = true; + //_flag_enable_mc_motors = true; break; @@ -114,7 +114,7 @@ void Kitepower::update_vtol_state() //Kitepower: direct switch for the moment for transition _vtol_schedule.flight_mode = MC_MODE; - _flag_enable_mc_motors = true; + //_flag_enable_mc_motors = true; break; } @@ -125,23 +125,23 @@ void Kitepower::update_vtol_state() // initialise a front transition _vtol_schedule.flight_mode = TRANSITION_FRONT; _vtol_schedule.transition_start = hrt_absolute_time(); - _flag_enable_mc_motors = false; + //_flag_enable_mc_motors = false; break; case FW_MODE: - _flag_enable_mc_motors = false; + //_flag_enable_mc_motors = false; break; case TRANSITION_FRONT: //Kitpower: direct switch for the moment for front transition _vtol_schedule.flight_mode = FW_MODE; - _flag_enable_mc_motors = false; + //_flag_enable_mc_motors = false; break; case TRANSITION_BACK: // failsafe into fixed wing mode _vtol_schedule.flight_mode = FW_MODE; - _flag_enable_mc_motors = false; + //_flag_enable_mc_motors = false; break; } @@ -155,13 +155,13 @@ void Kitepower::update_external_VTOL_state() case MC_MODE: _vtol_mode = ROTARY_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; + _flag_was_in_trans_mode = false; break; case FW_MODE: _vtol_mode = FIXED_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; + _flag_was_in_trans_mode = false; break; case TRANSITION_FRONT: @@ -286,10 +286,11 @@ void Kitepower::update_mc_state() _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; + /*for the moment let the motor spin during fixed wing, nice for simulation if (_flag_enable_mc_motors) { set_max_mc(1940); _flag_enable_mc_motors = false; - } + }*/ } @@ -301,10 +302,11 @@ void Kitepower::update_fw_state() _mc_yaw_weight = 0.0f; _mc_throttle_weight = 0.0f; + /*for the moment let the motor spin during fixed wing, nice for simulation if (!_flag_enable_mc_motors) { set_max_mc(940); _flag_enable_mc_motors = true; - } + }*/ } @@ -339,9 +341,11 @@ void Kitepower::fill_actuator_outputs() } + /** * Disable all multirotor motors when in fw mode. */ +/*for the moment let the motor spin during fixed wing, nice for simulation void Kitepower::set_max_mc(unsigned pwm_value) { @@ -370,4 +374,4 @@ Kitepower::set_max_mc(unsigned pwm_value) } px4_close(fd); -} +}*/ From 5f79220374a0166edf501a9fcfd8e3fc6e58ae53 Mon Sep 17 00:00:00 2001 From: gkoenig Date: Tue, 10 Apr 2018 16:17:49 +0200 Subject: [PATCH 28/29] inverted parachute output signal --- ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix index 9bbc8eb4d350..8b625846017f 100644 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix @@ -33,7 +33,6 @@ Z: 8. main output: Parachute Release ------------------ Using control group to -M: 2 -O: 10000 10000 0 -10000 10000 -S: 2 7 10000 10000 0 -10000 10000 +M: 1 +O: -10000 -10000 0 10000 -10000 S: 3 5 -10000 -10000 0 -10000 10000 From 69d106f47d7962c64330dc45f2525bb96ca8a656 Mon Sep 17 00:00:00 2001 From: Xander Gerrmann Date: Tue, 15 May 2018 15:28:19 +0200 Subject: [PATCH 29/29] Revert "Merge branch 'gamma_rigid_wing' into set_actuator_pr" This reverts commit b4eed22dd5dc6425701c68624a76f1202a1b03fc, reversing changes made to e303bae86b1b95f8e98c72687dd681a2803b10f7. --- .gitmodules | 10 +- .../init.d/13014_kitepower_aeolus250 | 22 - ROMFS/px4fmu_common/init.d/rcS | 6 - .../mixers/kitepower_aeolus250.main.mix | 38 -- msg/CMakeLists.txt | 180 +++++++ msg/actuator_armed.msg | 10 + msg/actuator_controls.msg | 18 + msg/actuator_direct.msg | 3 + msg/actuator_outputs.msg | 4 + msg/adc_report.msg | 2 + msg/airspeed.msg | 5 + msg/att_pos_mocap.msg | 9 + msg/battery_status.msg | 22 + msg/camera_capture.msg | 8 + msg/camera_trigger.msg | 3 + msg/collision_report.msg | 7 + msg/commander_state.msg | 19 + msg/cpuload.msg | 2 + msg/debug_key_value.msg | 3 + msg/debug_value.msg | 3 + msg/debug_vect.msg | 5 + msg/differential_pressure.msg | 5 + msg/distance_sensor.msg | 23 + msg/ekf2_innovations.msg | 18 + msg/ekf2_timestamps.msg | 24 + msg/esc_report.msg | 11 + msg/esc_status.msg | 19 + msg/estimator_status.msg | 107 ++++ msg/follow_target.msg | 7 + msg/fw_pos_ctrl_status.msg | 13 + msg/geofence_result.msg | 10 + msg/gps_dump.msg | 6 + msg/gps_inject_data.msg | 3 + msg/home_position.msg | 16 + msg/input_rc.msg | 27 + msg/irlock_report.msg | 9 + msg/landing_target_innovations.msg | 7 + msg/landing_target_pose.msg | 24 + msg/led_control.msg | 33 ++ msg/log_message.msg | 5 + msg/manual_control_setpoint.msg | 65 +++ msg/mavlink_log.msg | 3 + msg/mission.msg | 4 + msg/mission_result.msg | 17 + msg/mount_orientation.msg | 1 + msg/multirotor_motor_limits.msg | 13 + msg/obstacle_distance.msg | 14 + msg/offboard_control_mode.msg | 9 + msg/optical_flow.msg | 15 + msg/parameter_update.msg | 3 + msg/position_setpoint.msg | 53 ++ msg/position_setpoint_triplet.msg | 6 + msg/power_button_state.msg | 8 + msg/pwm_input.msg | 4 + msg/qshell_req.msg | 3 + msg/rate_ctrl_status.msg | 11 + msg/rc_channels.msg | 34 ++ msg/rc_parameter_map.msg | 10 + msg/ros/actuator_controls_0.msg | 4 + msg/ros/actuator_controls_virtual_mc.msg | 4 + msg/safety.msg | 4 + msg/satellite_info.msg | 8 + msg/sensor_accel.msg | 18 + msg/sensor_baro.msg | 5 + msg/sensor_bias.msg | 30 ++ msg/sensor_combined.msg | 24 + msg/sensor_correction.msg | 55 ++ msg/sensor_gyro.msg | 18 + msg/sensor_mag.msg | 15 + msg/sensor_preflight.msg | 7 + msg/sensor_selection.msg | 9 + msg/servorail_status.msg | 2 + msg/subsystem_info.msg | 22 + msg/system_power.msg | 19 + msg/task_stack_info.msg | 6 + msg/tecs_status.msg | 27 + msg/telemetry_status.msg | 19 + msg/templates/px4/ros/msg.h.template | 95 ++++ msg/templates/px4/uorb/msg.h.template | 100 ++++ msg/templates/uorb/msg.cpp.template | 75 +++ msg/templates/uorb/msg.h.template | 135 +++++ msg/templates/uorb/uORBTopics.cpp.template | 73 +++ .../microRTPS_client.cpp.template | 225 ++++++++ msg/templates/uorb_microcdr/msg.cpp.template | 164 ++++++ msg/templates/uorb_microcdr/msg.h.template | 75 +++ .../uorb_microcdr/uORBTopics.cpp.template | 25 +- msg/templates/urtps/Publisher.cpp.template | 154 ++++++ msg/templates/urtps/Publisher.h.template | 92 ++++ msg/templates/urtps/RtpsTopics.cpp.template | 159 ++++++ msg/templates/urtps/RtpsTopics.h.template | 95 ++++ msg/templates/urtps/Subscriber.cpp.template | 146 +++++ msg/templates/urtps/Subscriber.h.template | 99 ++++ .../urtps/microRTPS_agent.cpp.template | 310 +++++++++++ .../microRTPS_agent_CMakeLists.txt.template | 54 ++ msg/templates/urtps/microRTPS_transport.cpp | 503 ++++++++++++++++++ msg/templates/urtps/microRTPS_transport.h | 133 +++++ msg/templates/urtps/msg.idl.template | 75 +++ msg/test_motor.msg | 4 + msg/time_offset.msg | 1 + msg/tools/gencpp | 1 + msg/tools/generate_microRTPS_bridge.py | 237 +++++++++ msg/tools/genmsg | 1 + msg/tools/px_generate_uorb_topic_files.py | 440 +++++++++++++++ msg/tools/px_generate_uorb_topic_helper.py | 201 +++++++ msg/tools/uorb_rtps_message_ids.py | 108 ++++ msg/transponder_report.msg | 24 + msg/tune_control.msg | 27 + msg/uavcan_parameter_request.msg | 8 + msg/uavcan_parameter_value.msg | 8 + msg/ulog_stream.msg | 16 + msg/ulog_stream_ack.msg | 7 + msg/vehicle_attitude.msg | 11 + msg/vehicle_attitude_setpoint.msg | 27 + msg/vehicle_command.msg | 113 ++++ msg/vehicle_command_ack.msg | 23 + msg/vehicle_control_mode.msg | 24 + msg/vehicle_global_position.msg | 36 ++ msg/vehicle_gps_position.msg | 31 ++ msg/vehicle_land_detected.msg | 5 + msg/vehicle_local_position.msg | 62 +++ msg/vehicle_local_position_setpoint.msg | 12 + msg/vehicle_rates_setpoint.msg | 7 + msg/vehicle_roi.msg | 21 + msg/vehicle_status.msg | 72 +++ msg/vehicle_status_flags.msg | 35 ++ msg/vtol_vehicle_status.msg | 12 + msg/wind_estimate.msg | 5 + .../nuttx-configs/aerofc-v1/nsh/defconfig | 38 -- platforms/posix/cmake/sitl_target.cmake | 6 - posix-configs/SITL/init/ekf2/rigid_wing | 96 ---- src/drivers/aerofc_adc/aerofc_adc.cpp | 24 - src/drivers/boards/aerofc-v1/aerofc_init.c | 31 -- src/drivers/ll40ls/ll40ls.cpp | 421 --------------- src/lib/mixer/mixer_multirotor.cpp | 127 ----- src/modules/commander/commander.cpp | 33 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../FixedwingPositionControl.cpp | 52 +- src/modules/systemlib/mixer/multi_tables.py | 278 ---------- src/modules/vtol_att_control/CMakeLists.txt | 1 - src/modules/vtol_att_control/kitepower.cpp | 377 ------------- src/modules/vtol_att_control/kitepower.h | 126 ----- .../vtol_att_control_main.cpp | 3 +- .../vtol_att_control/vtol_att_control_main.h | 1 - .../vtol_att_control_params.c | 92 +--- src/modules/vtol_att_control/vtol_type.h | 5 +- 145 files changed, 5609 insertions(+), 1785 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 delete mode 100644 ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix create mode 100644 msg/CMakeLists.txt create mode 100644 msg/actuator_armed.msg create mode 100644 msg/actuator_controls.msg create mode 100644 msg/actuator_direct.msg create mode 100644 msg/actuator_outputs.msg create mode 100644 msg/adc_report.msg create mode 100644 msg/airspeed.msg create mode 100644 msg/att_pos_mocap.msg create mode 100644 msg/battery_status.msg create mode 100644 msg/camera_capture.msg create mode 100644 msg/camera_trigger.msg create mode 100644 msg/collision_report.msg create mode 100644 msg/commander_state.msg create mode 100644 msg/cpuload.msg create mode 100644 msg/debug_key_value.msg create mode 100644 msg/debug_value.msg create mode 100644 msg/debug_vect.msg create mode 100644 msg/differential_pressure.msg create mode 100644 msg/distance_sensor.msg create mode 100644 msg/ekf2_innovations.msg create mode 100644 msg/ekf2_timestamps.msg create mode 100644 msg/esc_report.msg create mode 100644 msg/esc_status.msg create mode 100644 msg/estimator_status.msg create mode 100644 msg/follow_target.msg create mode 100644 msg/fw_pos_ctrl_status.msg create mode 100644 msg/geofence_result.msg create mode 100644 msg/gps_dump.msg create mode 100644 msg/gps_inject_data.msg create mode 100644 msg/home_position.msg create mode 100644 msg/input_rc.msg create mode 100644 msg/irlock_report.msg create mode 100644 msg/landing_target_innovations.msg create mode 100644 msg/landing_target_pose.msg create mode 100644 msg/led_control.msg create mode 100644 msg/log_message.msg create mode 100644 msg/manual_control_setpoint.msg create mode 100644 msg/mavlink_log.msg create mode 100644 msg/mission.msg create mode 100644 msg/mission_result.msg create mode 100644 msg/mount_orientation.msg create mode 100644 msg/multirotor_motor_limits.msg create mode 100644 msg/obstacle_distance.msg create mode 100644 msg/offboard_control_mode.msg create mode 100644 msg/optical_flow.msg create mode 100644 msg/parameter_update.msg create mode 100644 msg/position_setpoint.msg create mode 100644 msg/position_setpoint_triplet.msg create mode 100644 msg/power_button_state.msg create mode 100644 msg/pwm_input.msg create mode 100644 msg/qshell_req.msg create mode 100644 msg/rate_ctrl_status.msg create mode 100644 msg/rc_channels.msg create mode 100644 msg/rc_parameter_map.msg create mode 100644 msg/ros/actuator_controls_0.msg create mode 100644 msg/ros/actuator_controls_virtual_mc.msg create mode 100644 msg/safety.msg create mode 100644 msg/satellite_info.msg create mode 100644 msg/sensor_accel.msg create mode 100644 msg/sensor_baro.msg create mode 100644 msg/sensor_bias.msg create mode 100644 msg/sensor_combined.msg create mode 100644 msg/sensor_correction.msg create mode 100644 msg/sensor_gyro.msg create mode 100644 msg/sensor_mag.msg create mode 100644 msg/sensor_preflight.msg create mode 100644 msg/sensor_selection.msg create mode 100644 msg/servorail_status.msg create mode 100644 msg/subsystem_info.msg create mode 100644 msg/system_power.msg create mode 100644 msg/task_stack_info.msg create mode 100644 msg/tecs_status.msg create mode 100644 msg/telemetry_status.msg create mode 100644 msg/templates/px4/ros/msg.h.template create mode 100644 msg/templates/px4/uorb/msg.h.template create mode 100644 msg/templates/uorb/msg.cpp.template create mode 100644 msg/templates/uorb/msg.h.template create mode 100644 msg/templates/uorb/uORBTopics.cpp.template create mode 100644 msg/templates/uorb_microcdr/microRTPS_client.cpp.template create mode 100644 msg/templates/uorb_microcdr/msg.cpp.template create mode 100644 msg/templates/uorb_microcdr/msg.h.template rename src/modules/vtol_att_control/kitepower_params.c => msg/templates/uorb_microcdr/uORBTopics.cpp.template (78%) create mode 100644 msg/templates/urtps/Publisher.cpp.template create mode 100644 msg/templates/urtps/Publisher.h.template create mode 100644 msg/templates/urtps/RtpsTopics.cpp.template create mode 100644 msg/templates/urtps/RtpsTopics.h.template create mode 100644 msg/templates/urtps/Subscriber.cpp.template create mode 100644 msg/templates/urtps/Subscriber.h.template create mode 100644 msg/templates/urtps/microRTPS_agent.cpp.template create mode 100644 msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template create mode 100644 msg/templates/urtps/microRTPS_transport.cpp create mode 100644 msg/templates/urtps/microRTPS_transport.h create mode 100644 msg/templates/urtps/msg.idl.template create mode 100644 msg/test_motor.msg create mode 100644 msg/time_offset.msg create mode 160000 msg/tools/gencpp create mode 100644 msg/tools/generate_microRTPS_bridge.py create mode 160000 msg/tools/genmsg create mode 100755 msg/tools/px_generate_uorb_topic_files.py create mode 100644 msg/tools/px_generate_uorb_topic_helper.py create mode 100644 msg/tools/uorb_rtps_message_ids.py create mode 100644 msg/transponder_report.msg create mode 100644 msg/tune_control.msg create mode 100644 msg/uavcan_parameter_request.msg create mode 100644 msg/uavcan_parameter_value.msg create mode 100644 msg/ulog_stream.msg create mode 100644 msg/ulog_stream_ack.msg create mode 100644 msg/vehicle_attitude.msg create mode 100644 msg/vehicle_attitude_setpoint.msg create mode 100644 msg/vehicle_command.msg create mode 100644 msg/vehicle_command_ack.msg create mode 100644 msg/vehicle_control_mode.msg create mode 100644 msg/vehicle_global_position.msg create mode 100644 msg/vehicle_gps_position.msg create mode 100644 msg/vehicle_land_detected.msg create mode 100644 msg/vehicle_local_position.msg create mode 100644 msg/vehicle_local_position_setpoint.msg create mode 100644 msg/vehicle_rates_setpoint.msg create mode 100644 msg/vehicle_roi.msg create mode 100644 msg/vehicle_status.msg create mode 100644 msg/vehicle_status_flags.msg create mode 100644 msg/vtol_vehicle_status.msg create mode 100644 msg/wind_estimate.msg delete mode 100644 posix-configs/SITL/init/ekf2/rigid_wing delete mode 100644 src/drivers/ll40ls/ll40ls.cpp delete mode 100755 src/modules/systemlib/mixer/multi_tables.py delete mode 100644 src/modules/vtol_att_control/kitepower.cpp delete mode 100644 src/modules/vtol_att_control/kitepower.h diff --git a/.gitmodules b/.gitmodules index dd06317c4e5c..74e658d905d3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,17 +2,14 @@ path = mavlink/include/mavlink/v2.0 url = https://github.com/mavlink/c_library_v2.git branch = master -[submodule "NuttX"] - path = NuttX - url = https://github.com/PX4/PX4NuttX.git [submodule "src/modules/uavcan/libuavcan"] path = src/modules/uavcan/libuavcan url = https://github.com/UAVCAN/libuavcan.git [submodule "Tools/genmsg"] - path = Tools/genmsg + path = msg/tools/genmsg url = https://github.com/ros/genmsg.git [submodule "Tools/gencpp"] - path = Tools/gencpp + path = msg/tools/gencpp url = https://github.com/ros/gencpp.git [submodule "Tools/jMAVSim"] path = Tools/jMAVSim @@ -20,7 +17,8 @@ branch = master [submodule "Tools/sitl_gazebo"] path = Tools/sitl_gazebo - url = git@192.168.11.131:rigidWing/sitl_gazebo.git + url = https://github.com/PX4/sitl_gazebo.git + branch = master [submodule "src/lib/matrix"] path = src/lib/matrix url = https://github.com/PX4/Matrix.git diff --git a/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 b/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 deleted file mode 100644 index 4725af2a984e..000000000000 --- a/ROMFS/px4fmu_common/init.d/13014_kitepower_aeolus250 +++ /dev/null @@ -1,22 +0,0 @@ -#!nsh -# -# @name VTOL Aeolus250 -# @class VTOL -# -# @type VTOL Quad Tailsitter -# -# @output MAIN5 RUDDER -# @output MAIN6 ELEVATOR -# -# @maintainer Gabriel Koenig -# - -sh /etc/init.d/rc.vtol_defaults - -set MIXER kitepower_aeolus250 - -set PWM_OUT 1234 -set MAV_TYPE 20 -set VT_TYPE 3 -set PWM_MIN 1100 -set CBRK_USB_CHK 197848 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 87f7feb46632..a136dc4a888e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -563,13 +563,7 @@ then fi if param greater SENS_EN_LEDDAR1 0 then -<<<<<<< HEAD set MAVLINK_F none -||||||| merged common ancestors - set MAVLINK_F "-r 1200 -d /dev/ttyS4" -======= - set MAVLINK_F "-r 1200 -d /dev/ttyS3" ->>>>>>> gamma_rigid_wing fi fi diff --git a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix b/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix deleted file mode 100644 index 8b625846017f..000000000000 --- a/ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix +++ /dev/null @@ -1,38 +0,0 @@ -kitepower aeolus mixer -============================ - -This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle -has two motors in total, one attached to each wing. It also has two elevons which -are located in the slipstream of the propellers. This mixer generates 4 PWM outputs -on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the -elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run -at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. - -Motor mixer ------------- -Channel 1 connects to the right (starboard) motor. -Channel 2 connects to the left (port) motor. - -R: 4aeol 10000 10000 10000 0 - -5. main output: Using Control Group 1 (VTOL) pitch ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 - -6. main output: using Control Group 1 (VTOL) yaw ------------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 2 10000 10000 0 -10000 10000 - -Channel 7 is null -Z: - -8. main output: Parachute Release ------------------- -Using control group to -M: 1 -O: -10000 -10000 0 10000 -10000 -S: 3 5 -10000 -10000 0 -10000 10000 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt new file mode 100644 index 000000000000..851c781fa471 --- /dev/null +++ b/msg/CMakeLists.txt @@ -0,0 +1,180 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(msg_files + actuator_armed.msg + actuator_controls.msg + actuator_direct.msg + actuator_outputs.msg + adc_report.msg + airspeed.msg + att_pos_mocap.msg + battery_status.msg + camera_capture.msg + camera_trigger.msg + collision_report.msg + commander_state.msg + cpuload.msg + debug_key_value.msg + debug_value.msg + debug_vect.msg + differential_pressure.msg + distance_sensor.msg + ekf2_innovations.msg + ekf2_timestamps.msg + esc_report.msg + esc_status.msg + estimator_status.msg + follow_target.msg + fw_pos_ctrl_status.msg + geofence_result.msg + gps_dump.msg + gps_inject_data.msg + home_position.msg + input_rc.msg + irlock_report.msg + landing_target_innovations.msg + landing_target_pose.msg + led_control.msg + log_message.msg + manual_control_setpoint.msg + mavlink_log.msg + mission.msg + mission_result.msg + mount_orientation.msg + multirotor_motor_limits.msg + obstacle_distance.msg + offboard_control_mode.msg + optical_flow.msg + parameter_update.msg + position_setpoint.msg + position_setpoint_triplet.msg + power_button_state.msg + pwm_input.msg + qshell_req.msg + rate_ctrl_status.msg + rc_channels.msg + rc_parameter_map.msg + safety.msg + satellite_info.msg + sensor_accel.msg + sensor_baro.msg + sensor_bias.msg + sensor_combined.msg + sensor_correction.msg + sensor_gyro.msg + sensor_mag.msg + sensor_preflight.msg + sensor_selection.msg + servorail_status.msg + subsystem_info.msg + system_power.msg + task_stack_info.msg + tecs_status.msg + telemetry_status.msg + test_motor.msg + time_offset.msg + transponder_report.msg + tune_control.msg + uavcan_parameter_request.msg + uavcan_parameter_value.msg + ulog_stream.msg + ulog_stream_ack.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + vehicle_command.msg + vehicle_command_ack.msg + vehicle_control_mode.msg + vehicle_global_position.msg + vehicle_gps_position.msg + vehicle_land_detected.msg + vehicle_local_position.msg + vehicle_local_position_setpoint.msg + vehicle_rates_setpoint.msg + vehicle_roi.msg + vehicle_status.msg + vehicle_status_flags.msg + vtol_vehicle_status.msg + wind_estimate.msg + ) + +px4_add_git_submodule(TARGET git_gencpp PATH tools/gencpp) +px4_add_git_submodule(TARGET git_genmsg PATH tools/genmsg) + +# headers +set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics) +set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) + +set(uorb_headers) +set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp) +foreach(msg_file ${msg_files}) + get_filename_component(msg ${msg_file} NAME_WE) + list(APPEND uorb_headers ${msg_out_path}/${msg}.h) + list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp) +endforeach() + +# Generate uORB headers +add_custom_command(OUTPUT ${uorb_headers} + COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py + --headers + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${msg_out_path} + -e templates/uorb + -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers + -q + DEPENDS ${msg_files} + COMMENT "Generating uORB topic headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) +add_custom_target(uorb_headers DEPENDS ${uorb_headers}) + +# Generate uORB sources +add_custom_command(OUTPUT ${uorb_sources} + COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py + --sources + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${msg_source_out_path} + -e templates/uorb + -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources + -q + DEPENDS ${msg_files} + COMMENT "Generating uORB topic sources" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + +px4_add_library(uorb_msgs ${uorb_sources}) +add_dependencies(uorb_msgs uorb_headers) diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg new file mode 100644 index 000000000000..f44ea729c280 --- /dev/null +++ b/msg/actuator_armed.msg @@ -0,0 +1,10 @@ + +uint32 armed_time_ms # Arming timestamp +bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool manual_lockdown # Set to true if manual throttle kill switch is engaged +bool force_failsafe # Set to true if the actuators are forced to the failsafe position +bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics +bool soft_stop # Set to true if we need to ESCs to remove the idle constraint diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg new file mode 100644 index 000000000000..46bf35ebff2f --- /dev/null +++ b/msg/actuator_controls.msg @@ -0,0 +1,18 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 +uint8 GROUP_INDEX_ATTITUDE = 0 +uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control + +# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 +# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc + diff --git a/msg/actuator_direct.msg b/msg/actuator_direct.msg new file mode 100644 index 000000000000..7d22f79e7550 --- /dev/null +++ b/msg/actuator_direct.msg @@ -0,0 +1,3 @@ +uint8 NUM_ACTUATORS_DIRECT = 16 +uint32 nvalues # number of valid values +float32[16] values # actuator values, from -1 to 1 diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg new file mode 100644 index 000000000000..269255340db1 --- /dev/null +++ b/msg/actuator_outputs.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_OUTPUTS = 16 +uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking +uint32 noutputs # valid outputs +float32[16] output # output data, in natural output units diff --git a/msg/adc_report.msg b/msg/adc_report.msg new file mode 100644 index 000000000000..7d43562dc587 --- /dev/null +++ b/msg/adc_report.msg @@ -0,0 +1,2 @@ +int16[12] channel_id # ADC channel IDs, negative for non-existent +float32[12] channel_value # ADC channel value in volt, valid if channel ID is positive diff --git a/msg/airspeed.msg b/msg/airspeed.msg new file mode 100644 index 000000000000..744f9d8830bf --- /dev/null +++ b/msg/airspeed.msg @@ -0,0 +1,5 @@ +float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown +float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown +float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown +float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg new file mode 100644 index 000000000000..6c007a6393d5 --- /dev/null +++ b/msg/att_pos_mocap.msg @@ -0,0 +1,9 @@ +uint32 id # ID of the estimator, commonly the component ID of the incoming message + +uint64 timestamp_received # timestamp when the estimate was received + +float32[4] q # Estimated attitude as quaternion + +float32 x # X position in meters in NED earth-fixed frame +float32 y # Y position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) diff --git a/msg/battery_status.msg b/msg/battery_status.msg new file mode 100644 index 000000000000..2fe836261a3c --- /dev/null +++ b/msg/battery_status.msg @@ -0,0 +1,22 @@ +float32 voltage_v # Battery voltage in volts, 0 if unknown +float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown +float32 current_a # Battery current in amperes, -1 if unknown +float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown +float32 discharged_mah # Discharged amount in mAh, -1 if unknown +float32 remaining # From 1 to 0, -1 if unknown +float32 scale # Power scaling factor, >= 1, or -1 if unknown +int32 cell_count # Number of cells +bool connected # Whether or not a battery is connected, based on a voltage threshold +bool system_source # Whether or not a this battery is the active power source for VDD_5V_IN +uint8 priority # Zerro based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 + +bool is_powering_off # Power off event imminent indication, false if unknown + + +uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active +uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage +uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required +uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely + +uint8 warning # current battery warning diff --git a/msg/camera_capture.msg b/msg/camera_capture.msg new file mode 100644 index 000000000000..3f94a854d124 --- /dev/null +++ b/msg/camera_capture.msg @@ -0,0 +1,8 @@ +uint64 timestamp_utc # Capture time in UTC / GPS time +uint32 seq # Image sequence number +float64 lat # Latitude in degrees (WGS84) +float64 lon # Longitude in degrees (WGS84) +float32 alt # Altitude (AMSL) +float32 ground_distance # Altitude above ground (meters) +float32[4] q # Attitude of the camera, zero rotation is facing towards front of vehicle +int8 result # 1 for success, 0 for failure, -1 if camera does not provide feedback diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg new file mode 100644 index 000000000000..b50d1a5a5eb8 --- /dev/null +++ b/msg/camera_trigger.msg @@ -0,0 +1,3 @@ +uint64 timestamp_utc # UTC timestamp + +uint32 seq # Image sequence number diff --git a/msg/collision_report.msg b/msg/collision_report.msg new file mode 100644 index 000000000000..726c1fb78635 --- /dev/null +++ b/msg/collision_report.msg @@ -0,0 +1,7 @@ +uint8 src +uint32 id +uint8 action +uint8 threat_level +float32 time_to_minimum_delta +float32 altitude_minimum_delta +float32 horizontal_minimum_delta diff --git a/msg/commander_state.msg b/msg/commander_state.msg new file mode 100644 index 000000000000..00b5f1acacaf --- /dev/null +++ b/msg/commander_state.msg @@ -0,0 +1,19 @@ +# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. +uint8 MAIN_STATE_MANUAL = 0 +uint8 MAIN_STATE_ALTCTL = 1 +uint8 MAIN_STATE_POSCTL = 2 +uint8 MAIN_STATE_AUTO_MISSION = 3 +uint8 MAIN_STATE_AUTO_LOITER = 4 +uint8 MAIN_STATE_AUTO_RTL = 5 +uint8 MAIN_STATE_ACRO = 6 +uint8 MAIN_STATE_OFFBOARD = 7 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_AUTO_TAKEOFF = 10 +uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 +uint8 MAIN_STATE_AUTO_PRECLAND = 13 +uint8 MAIN_STATE_MAX = 14 + + +uint8 main_state # main state machine diff --git a/msg/cpuload.msg b/msg/cpuload.msg new file mode 100644 index 000000000000..9381c05e8341 --- /dev/null +++ b/msg/cpuload.msg @@ -0,0 +1,2 @@ +float32 load # processor load from 0 to 1 +float32 ram_usage # RAM usage from 0 to 1 diff --git a/msg/debug_key_value.msg b/msg/debug_key_value.msg new file mode 100644 index 000000000000..023531e03888 --- /dev/null +++ b/msg/debug_key_value.msg @@ -0,0 +1,3 @@ +uint32 timestamp_ms # in milliseconds since system start +char[10] key # max. 10 characters as key / name +float32 value # the value to send as debug output diff --git a/msg/debug_value.msg b/msg/debug_value.msg new file mode 100644 index 000000000000..d8d3bfd8ffad --- /dev/null +++ b/msg/debug_value.msg @@ -0,0 +1,3 @@ +uint32 timestamp_ms # in milliseconds since system start +int8 ind # index of debug variable +float32 value # the value to send as debug output diff --git a/msg/debug_vect.msg b/msg/debug_vect.msg new file mode 100644 index 000000000000..2088a6f6196d --- /dev/null +++ b/msg/debug_vect.msg @@ -0,0 +1,5 @@ +uint64 timestamp_us # in microseconds since system start +char[10] name # max. 10 characters as key / name +float32 x # x value +float32 y # y value +float32 z # z value diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg new file mode 100644 index 000000000000..fff2a6da50b1 --- /dev/null +++ b/msg/differential_pressure.msg @@ -0,0 +1,5 @@ +uint64 error_count # Number of errors detected by driver +float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative) +float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading +float32 temperature # Temperature provided by sensor, -1000.0f if unknown +uint32 device_id # unique device ID for the sensor that does not change between power cycles diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg new file mode 100644 index 000000000000..bf9db74e7f64 --- /dev/null +++ b/msg/distance_sensor.msg @@ -0,0 +1,23 @@ +# DISTANCE_SENSOR message data + + +float32 min_distance # Minimum distance the sensor can measure (in m) +float32 max_distance # Maximum distance the sensor can measure (in m) +float32 current_distance # Current distance reading (in m) +float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings + +uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +uint8 id # Onboard ID of the sensor + +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum +uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270 +uint8 ROTATION_UPWARD_FACING = 24 # MAV_SENSOR_ROTATION_PITCH_90 +uint8 ROTATION_BACKWARD_FACING = 12 # MAV_SENSOR_ROTATION_PITCH_180 +uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE +uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270 +uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90 diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg new file mode 100644 index 000000000000..85fd48b89ff3 --- /dev/null +++ b/msg/ekf2_innovations.msg @@ -0,0 +1,18 @@ +float32[6] vel_pos_innov # velocity and position innovations +float32[3] mag_innov # earth magnetic field innovations +float32 heading_innov # heading innovation +float32 airspeed_innov # airspeed innovation +float32 beta_innov # synthetic sideslip innovation +float32[2] flow_innov # flow innovation +float32 hagl_innov # innovation from the terrain estimator for the height above ground level measurement (m) +float32[6] vel_pos_innov_var # velocity and position innovation variances +float32[3] mag_innov_var # earth magnetic field innovation variance +float32 heading_innov_var # heading innovation variance +float32 airspeed_innov_var # Airspeed innovation variance +float32 beta_innov_var # synthetic sideslip innovation variance +float32[2] flow_innov_var # flow innovation variance +float32 hagl_innov_var # innovation variance from the terrain estimator for the height above ground level measurement (m^2) +float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) +float32[2] drag_innov # drag specific force innovation (m/s/s) +float32[2] drag_innov_var # drag specific force innovation variance (m/s/s)**2 +float32[2] aux_vel_innov # auxiliary NE velocity innovations from landing target measurement (m/s) diff --git a/msg/ekf2_timestamps.msg b/msg/ekf2_timestamps.msg new file mode 100644 index 000000000000..1f8a5f3a0ad7 --- /dev/null +++ b/msg/ekf2_timestamps.msg @@ -0,0 +1,24 @@ +# this message contains the (relative) timestamps of the sensor inputs used by +# ekf2. It can be used for reproducible replay. + +# the timestamp field (auto-generated) is the ekf2 reference time and matches +# the timestamp of the sensor_combined topic. + + +int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative timestamps + # is set to this value, it means the associated sensor values did not update + +# timestamps are relative to the main timestamp and are in 0.1 ms (timestamp + +# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum +# difference of +-3.2s to the sensor_combined topic. + + +int16 gps_timestamp_rel +int16 optical_flow_timestamp_rel +int16 distance_sensor_timestamp_rel +int16 airspeed_timestamp_rel +int16 vision_position_timestamp_rel +int16 vision_attitude_timestamp_rel + +# Note: this is a high-rate logged topic, so it needs to be as small as possible + diff --git a/msg/esc_report.msg b/msg/esc_report.msg new file mode 100644 index 000000000000..8525f3be1532 --- /dev/null +++ b/msg/esc_report.msg @@ -0,0 +1,11 @@ +uint8 esc_vendor # Vendor of current ESC +uint32 esc_errorcount # Number of reported errors by ESC - if supported +int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported +float32 esc_voltage # Voltage measured from current ESC [V] - if supported +float32 esc_current # Current measured from current ESC [A] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +float32 esc_setpoint # setpoint of current ESC +uint16 esc_setpoint_raw # setpoint of current ESC (Value sent to ESC) +uint16 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint16 esc_version # Version of current ESC - if supported +uint16 esc_state # State of ESC - depend on Vendor diff --git a/msg/esc_status.msg b/msg/esc_status.msg new file mode 100644 index 000000000000..5148642459ff --- /dev/null +++ b/msg/esc_status.msg @@ -0,0 +1,19 @@ +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs + +uint8 ESC_VENDOR_GENERIC = 0 # generic ESC +uint8 ESC_VENDOR_MIKROKOPTER = 1 # Mikrokopter +uint8 ESC_VENDOR_GRAUPNER_HOTT = 2 # Graupner HoTT ESC +uint8 ESC_VENDOR_TAP = 3 # TAP ESC + +uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC +uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC +uint8 ESC_CONNECTION_TYPE_ONESHOOT = 2 # One Shoot PPM +uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C +uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus + +uint16 counter # incremented by the writing thread everytime new data is stored + +uint8 esc_count # number of connected ESCs +uint8 esc_connectiontype # how ESCs connected to the system + +esc_report[8] esc diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg new file mode 100644 index 000000000000..94b523cd2082 --- /dev/null +++ b/msg/estimator_status.msg @@ -0,0 +1,107 @@ +float32[24] states # Internal filter states +float32 n_states # Number of states effectively used + +float32[3] vibe # IMU vibration metrics in the following array locations +# 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) +# 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) +# 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + +uint8 nan_flags # Bitmask to indicate NaN states +uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) +uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) +float32[24] covariances # Diagonal Elements of Covariance Matrix + +uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below +# bits are true when corresponding test has failed +uint16 GPS_CHECK_FAIL_GPS_FIX = 0 # 0 : insufficient fix type (no 3D solution) +uint16 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1 # 1 : minimum required sat count fail +uint16 GPS_CHECK_FAIL_MIN_GDOP = 2 # 2 : minimum required GDoP fail +uint16 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3 # 3 : maximum allowed horizontal position error fail +uint16 GPS_CHECK_FAIL_MAX_VERT_ERR = 4 # 4 : maximum allowed vertical position error fail +uint16 GPS_CHECK_FAIL_MAX_SPD_ERR = 5 # 5 : maximum allowed speed error fail +uint16 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6 # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +uint16 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +uint16 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +uint16 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail + +uint32 control_mode_flags # Bitmask to indicate EKF logic state +# 0 - true if the filter tilt alignment is complete +# 1 - true if the filter yaw alignment is complete +# 2 - true if GPS measurements are being fused +# 3 - true if optical flow measurements are being fused +# 4 - true if a simple magnetic yaw heading is being fused +# 5 - true if 3-axis magnetometer measurement are being fused +# 6 - true if synthetic magnetic declination measurements are being fused +# 7 - true when the vehicle is airborne +# 8 - true when wind velocity is being estimated +# 9 - true when baro height is being fused as a primary height reference +# 10 - true when range finder height is being fused as a primary height reference +# 11 - true when GPS height is being fused as a primary height reference +# 12 - true when local position data from external vision is being fused +# 13 - true when yaw data from external vision measurements is being fused +# 14 - true when height data from external vision measurements is being fused +# 15 - true when synthetic sideslip measurements are being fused +# 16 - true when only the magnetic field states are updated by the magnetometer +# 17 - true when the vehicle is operating as a fixed wing vehicle +# 18 - true when the magnetomer has been declared faulty and is no longer being used +# 19 - true when airspeed measurements are being fused + +uint16 filter_fault_flags # Bitmask to indicate EKF internal faults +# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +# 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +# 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +# 3 - true if the fusion of the magnetic heading has encountered a numerical error +# 4 - true if the fusion of the magnetic declination has encountered a numerical error +# 5 - true if fusion of the airspeed has encountered a numerical error +# 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +# 7 - true if fusion of the optical flow X axis has encountered a numerical error +# 8 - true if fusion of the optical flow Y axis has encountered a numerical error +# 9 - true if fusion of the North velocity has encountered a numerical error +# 10 - true if fusion of the East velocity has encountered a numerical error +# 11 - true if fusion of the Down velocity has encountered a numerical error +# 12 - true if fusion of the North position has encountered a numerical error +# 13 - true if fusion of the East position has encountered a numerical error +# 14 - true if fusion of the Down position has encountered a numerical error +# 15 - true if bad delta velocity bias estimates have been detected + +float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) +float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) +uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks +# 0 - true if velocity observations have been rejected +# 1 - true if horizontal position observations have been rejected +# 2 - true if true if vertical position observations have been rejected +# 3 - true if the X magnetometer observation has been rejected +# 4 - true if the Y magnetometer observation has been rejected +# 5 - true if the Z magnetometer observation has been rejected +# 6 - true if the yaw observation has been rejected +# 7 - true if the airspeed observation has been rejected +# 8 - true if the synthetic sideslip observation has been rejected +# 9 - true if the height above ground observation has been rejected +# 10 - true if the X optical flow observation has been rejected +# 11 - true if the Y optical flow observation has been rejected + +float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit +float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit +float32 pos_test_ratio # ratio of the largest horizontal position innovation component to the innovation test limit +float32 hgt_test_ratio # ratio of the vertical position innovation to the innovation test limit +float32 tas_test_ratio # ratio of the true airspeed innovation to the innovation test limit +float32 hagl_test_ratio # ratio of the height above ground innovation to the innovation test limit +float32 beta_test_ratio # ratio of the synthetic sideslip innovation to the innovation test limit + +uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use. +# 0 - True if the attitude estimate is good +# 1 - True if the horizontal velocity estimate is good +# 2 - True if the vertical velocity estimate is good +# 3 - True if the horizontal position (relative) estimate is good +# 4 - True if the horizontal position (absolute) estimate is good +# 5 - True if the vertical position (absolute) estimate is good +# 6 - True if the vertical position (above ground) estimate is good +# 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) +# 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate +# 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate +# 10 - True if the EKF has detected a GPS glitch +# 11 - True if the EKF has detected bad accelerometer data + +float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time + +bool pre_flt_fail # true when estimator has failed pre-flight checks and the vehicle should not be flown regardless of flight mode diff --git a/msg/follow_target.msg b/msg/follow_target.msg new file mode 100644 index 000000000000..75f124e2ee4b --- /dev/null +++ b/msg/follow_target.msg @@ -0,0 +1,7 @@ +float64 lat # target position (deg * 1e7) +float64 lon # target position (deg * 1e7) +float32 alt # target position +float32 vy # target vel in y +float32 vx # target vel in x +float32 vz # target vel in z +uint8 est_cap # target reporting capabilities diff --git a/msg/fw_pos_ctrl_status.msg b/msg/fw_pos_ctrl_status.msg new file mode 100644 index 000000000000..0af57b1bf5fc --- /dev/null +++ b/msg/fw_pos_ctrl_status.msg @@ -0,0 +1,13 @@ +float32 nav_roll +float32 nav_pitch +float32 nav_bearing + +float32 target_bearing +float32 wp_dist +float32 xtrack_error +float32 turn_distance # the optimal distance to a waypoint to switch to the next + +float32 landing_horizontal_slope_displacement +float32 landing_slope_angle_rad +float32 landing_flare_length +bool abort_landing diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg new file mode 100644 index 000000000000..0daba4a4ac22 --- /dev/null +++ b/msg/geofence_result.msg @@ -0,0 +1,10 @@ +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination + +bool geofence_violated # true if the geofence is violated +uint8 geofence_action # action to take when geofence is violated + +bool home_required # true if the geofence requires a valid home position diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg new file mode 100644 index 000000000000..8a91149d46aa --- /dev/null +++ b/msg/gps_dump.msg @@ -0,0 +1,6 @@ +# This message is used to dump the raw gps communication to the log. +# Set the parameter GPS_DUMP_COMM to 1 to use this. + +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg new file mode 100644 index 000000000000..ac44d02dbeea --- /dev/null +++ b/msg/gps_inject_data.msg @@ -0,0 +1,3 @@ +uint8 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[182] data # data to write to GPS device (RTCM message) diff --git a/msg/home_position.msg b/msg/home_position.msg new file mode 100644 index 000000000000..5515aadfb799 --- /dev/null +++ b/msg/home_position.msg @@ -0,0 +1,16 @@ +# GPS home position in WGS84 coordinates. + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians + +bool valid_alt # true when the altitude has been set +bool valid_hpos # true when the latitude and longitude have been set + +bool manual_home # true when home position was set manually diff --git a/msg/input_rc.msg b/msg/input_rc.msg new file mode 100644 index 000000000000..4df163fb490a --- /dev/null +++ b/msg/input_rc.msg @@ -0,0 +1,27 @@ +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 +uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 +uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 +uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 +uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 +uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 +uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_last_signal # last valid reception time +uint32 channel_count # number of channels actually being seen +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usUally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels diff --git a/msg/irlock_report.msg b/msg/irlock_report.msg new file mode 100644 index 000000000000..ded99a258163 --- /dev/null +++ b/msg/irlock_report.msg @@ -0,0 +1,9 @@ +# IRLOCK_REPORT message data + +uint16 signature + +# When looking along the optical axis of the camera, x points right, y points down, and z points along the optical axis. +float32 pos_x # tan(theta), where theta is the angle between the target and the camera center of projection in camera x-axis +float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis +float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/ +float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/ diff --git a/msg/landing_target_innovations.msg b/msg/landing_target_innovations.msg new file mode 100644 index 000000000000..73286aa59554 --- /dev/null +++ b/msg/landing_target_innovations.msg @@ -0,0 +1,7 @@ +# Innovation of landing target position estimator +float32 innov_x +float32 innov_y + +# Innovation covariance of landing target position estimator +float32 innov_cov_x +float32 innov_cov_y diff --git a/msg/landing_target_pose.msg b/msg/landing_target_pose.msg new file mode 100644 index 000000000000..5a31fa953fb2 --- /dev/null +++ b/msg/landing_target_pose.msg @@ -0,0 +1,24 @@ +# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames + +bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground + +bool rel_pos_valid # Flag showing whether relative position is valid +bool rel_vel_valid # Flag showing whether relative velocity is valid + +float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters] +float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters] +float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters] + +float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] + +float32 cov_x_rel # X/north position variance [meters^2] +float32 cov_y_rel # Y/east position variance [meters^2] + +float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] +float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] + +bool abs_pos_valid # Flag showing whether absolute position is valid +float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] +float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters] +float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters] diff --git a/msg/led_control.msg b/msg/led_control.msg new file mode 100644 index 000000000000..1a0bb6f06bbd --- /dev/null +++ b/msg/led_control.msg @@ -0,0 +1,33 @@ +# LED control: control a single or multiple LED's. +# These are the externally visible LED's, not the board LED's + +# colors +uint8 COLOR_OFF = 0 # this is only used in the drivers +uint8 COLOR_RED = 1 +uint8 COLOR_GREEN = 2 +uint8 COLOR_BLUE = 3 +uint8 COLOR_YELLOW = 4 +uint8 COLOR_PURPLE = 5 +uint8 COLOR_AMBER = 6 +uint8 COLOR_CYAN = 7 +uint8 COLOR_WHITE = 8 + +# LED modes definitions +uint8 MODE_OFF = 0 # turn LED off +uint8 MODE_ON = 1 # turn LED on +uint8 MODE_DISABLED = 2 # disable this priority (switch to lower priority setting) +uint8 MODE_BLINK_SLOW = 3 +uint8 MODE_BLINK_NORMAL = 4 +uint8 MODE_BLINK_FAST = 5 +uint8 MODE_BREATHE = 6 # continuously increase & decrease brightness (solid color if driver does not support it) +uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAST and then off for a while + +uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0) + + +uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all +uint8 color # see COLOR_* +uint8 mode # see MODE_* +uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite + # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 +uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) diff --git a/msg/log_message.msg b/msg/log_message.msg new file mode 100644 index 000000000000..51fcbbd50d3a --- /dev/null +++ b/msg/log_message.msg @@ -0,0 +1,5 @@ +# A logging message, output with PX4_{WARN,ERR,INFO} + +uint8 severity # log level (same as in the linux kernel, starting with 0) +uint8[127] text + diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg new file mode 100644 index 000000000000..253aa327a8ee --- /dev/null +++ b/msg/manual_control_setpoint.msg @@ -0,0 +1,65 @@ + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) +int8 MODE_SLOT_NONE = -1 # no mode slot assigned +int8 MODE_SLOT_1 = 0 # mode slot 1 selected +int8 MODE_SLOT_2 = 1 # mode slot 2 selected +int8 MODE_SLOT_3 = 2 # mode slot 3 selected +int8 MODE_SLOT_4 = 3 # mode slot 4 selected +int8 MODE_SLOT_5 = 4 # mode slot 5 selected +int8 MODE_SLOT_6 = 5 # mode slot 6 selected +int8 MODE_SLOT_MAX = 6 # number of slots plus one +uint8 SOURCE_RC = 1 # radio control +uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 +uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 +uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 4 + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down +float32 r # yaw stick/twist position, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle +float32 flaps # flap position +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop + +uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE +uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL +uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED +uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT +uint8 gear_switch # landing gear switch: _DOWN_, UP +int8 mode_slot # the slot a specific model selector is in +uint8 data_source # where this input is coming from +uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED +uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg new file mode 100644 index 000000000000..274596adde59 --- /dev/null +++ b/msg/mavlink_log.msg @@ -0,0 +1,3 @@ + +uint8[50] text +uint8 severity # log level (same as in the linux kernel, starting with 0) diff --git a/msg/mission.msg b/msg/mission.msg new file mode 100644 index 000000000000..c2fbaafabcd5 --- /dev/null +++ b/msg/mission.msg @@ -0,0 +1,4 @@ +uint8 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 + +uint16 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest \ No newline at end of file diff --git a/msg/mission_result.msg b/msg/mission_result.msg new file mode 100644 index 000000000000..a780ba8463b9 --- /dev/null +++ b/msg/mission_result.msg @@ -0,0 +1,17 @@ +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified + +int32 seq_reached # Sequence of the mission item which has been reached, default -1 +uint16 seq_current # Sequence of the current mission item +uint16 seq_total # Total number of mission items + +bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings +bool finished # true if mission has been completed +bool failure # true if the mission cannot continue or be completed for some reason + +bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode +bool flight_termination # true if the navigator demands a flight termination from the commander app + +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint16 item_changed_index # indicate which item has changed +uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/msg/mount_orientation.msg b/msg/mount_orientation.msg new file mode 100644 index 000000000000..9d068bd168b8 --- /dev/null +++ b/msg/mount_orientation.msg @@ -0,0 +1 @@ +float32[3] attitude_euler_angle # Attitude/direction of the mount as euler angles in rad diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg new file mode 100644 index 000000000000..ba6c8f3de005 --- /dev/null +++ b/msg/multirotor_motor_limits.msg @@ -0,0 +1,13 @@ +uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated + +# 0 - True if the saturation status is valid +# 1 - True if any motor is saturated at the upper limit +# 2 - True if any motor is saturated at the lower limit +# 3 - True if a positive roll increment will increase motor saturation +# 4 - True if negative roll increment will increase motor saturation +# 5 - True if positive pitch increment will increase motor saturation +# 6 - True if negative pitch increment will increase motor saturation +# 7 - True if positive yaw increment will increase motor saturation +# 8 - True if negative yaw increment will increase motor saturation +# 9 - True if positive thrust increment will increase motor saturation +# 10 - True if negative thrust increment will increase motor saturation diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg new file mode 100644 index 000000000000..a2cf3f968c61 --- /dev/null +++ b/msg/obstacle_distance.msg @@ -0,0 +1,14 @@ +# Obstacle distances in front of the sensor. + +uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum. +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +uint16[72] distances # Distance of obstacles in front of the sensor starting on the left side. A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstace is present. A value of UINT16_MAX for unknown/not used. In a array element, each unit corresponds to 1cm. + +uint8 increment # Angular width in degrees of each array element. + +uint16 min_distance # Minimum distance the sensor can measure in centimeters. +uint16 max_distance # Maximum distance the sensor can measure in centimeters. diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg new file mode 100644 index 000000000000..eaff918152d8 --- /dev/null +++ b/msg/offboard_control_mode.msg @@ -0,0 +1,9 @@ +# Off-board control mode + +bool ignore_thrust +bool ignore_attitude +bool ignore_bodyrate +bool ignore_position +bool ignore_velocity +bool ignore_acceleration_force +bool ignore_alt_hold diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg new file mode 100644 index 000000000000..568ca26e38a7 --- /dev/null +++ b/msg/optical_flow.msg @@ -0,0 +1,15 @@ +# Optical flow in XYZ body frame in SI units. +# @see http://en.wikipedia.org/wiki/International_System_of_Units + +uint8 sensor_id # id of the sensor emitting the flow value +float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis +float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis +float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis +float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis +float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis +float32 ground_distance_m # Altitude / distance to ground in meters +uint32 integration_timespan # accumulation timespan in microseconds +uint32 time_since_last_sonar_update # time since last sonar update in microseconds +uint16 frame_count_since_last_readout # number of accumulated frames in timespan +int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg new file mode 100644 index 000000000000..d7e88320f075 --- /dev/null +++ b/msg/parameter_update.msg @@ -0,0 +1,3 @@ +# This message is used to notify the system about one or more parameter changes + +uint32 instance # Instance count - constantly incrementing diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg new file mode 100644 index 000000000000..29a83ed715e9 --- /dev/null +++ b/msg/position_setpoint.msg @@ -0,0 +1,53 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard +uint8 SETPOINT_TYPE_FOLLOW_TARGET=7 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target + +uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED +uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller + +float32 x # local position setpoint in m in NED +float32 y # local position setpoint in m in NED +float32 z # local position setpoint in m in NED +bool position_valid # true if local position setpoint valid + +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED +bool velocity_valid # true if local velocity setpoint valid +uint8 velocity_frame # to set velocity setpoints in NED or body +bool alt_valid # do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control. + +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m + +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid + +float32 yawspeed # yawspeed (only for multirotors, in rad/s) +bool yawspeed_valid # true if yawspeed setpoint valid + +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints + +float32 a_x # acceleration x setpoint +float32 a_y # acceleration y setpoint +float32 a_z # acceleration z setpoint +bool acceleration_valid # true if acceleration setpoint is valid/should be used +bool acceleration_is_force # interprete acceleration as force + +float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation + +float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg new file mode 100644 index 000000000000..51ac92dfdfa6 --- /dev/null +++ b/msg/position_setpoint_triplet.msg @@ -0,0 +1,6 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +px4/position_setpoint previous +px4/position_setpoint current +px4/position_setpoint next diff --git a/msg/power_button_state.msg b/msg/power_button_state.msg new file mode 100644 index 000000000000..b941aa820c20 --- /dev/null +++ b/msg/power_button_state.msg @@ -0,0 +1,8 @@ +# power button state notification message + +uint8 PWR_BUTTON_STATE_IDEL = 0 # Button went up without meeting shutdown button down time (delete event) +uint8 PWR_BUTTON_STATE_DOWN = 1 # Button went Down +uint8 PWR_BUTTON_STATE_UP = 2 # Button went Up +uint8 PWR_BUTTON_STATE_REQUEST_SHUTDOWN = 3 # Button went Up after meeting shutdown button down time + +uint8 event # one of PWR_BUTTON_STATE_* diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg new file mode 100644 index 000000000000..8b91883532d4 --- /dev/null +++ b/msg/pwm_input.msg @@ -0,0 +1,4 @@ + +uint64 error_count +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg new file mode 100644 index 000000000000..fa75390edd55 --- /dev/null +++ b/msg/qshell_req.msg @@ -0,0 +1,3 @@ +int32[100] string +uint64 MAX_STRLEN = 100 +uint64 strlen diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg new file mode 100644 index 000000000000..0e28ede022bb --- /dev/null +++ b/msg/rate_ctrl_status.msg @@ -0,0 +1,11 @@ + +# rates used by the controller +float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s +float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s +float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s + +# rate controller integrator status +float32 rollspeed_integ +float32 pitchspeed_integ +float32 yawspeed_integ +float32 additional_integ1 # FW: wheel rate integrator (optional) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg new file mode 100644 index 000000000000..6d31e9d5a8de --- /dev/null +++ b/msg/rc_channels.msg @@ -0,0 +1,34 @@ +uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 +uint8 RC_CHANNELS_FUNCTION_ROLL=1 +uint8 RC_CHANNELS_FUNCTION_PITCH=2 +uint8 RC_CHANNELS_FUNCTION_YAW=3 +uint8 RC_CHANNELS_FUNCTION_MODE=4 +uint8 RC_CHANNELS_FUNCTION_RETURN=5 +uint8 RC_CHANNELS_FUNCTION_POSCTL=6 +uint8 RC_CHANNELS_FUNCTION_LOITER=7 +uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8 +uint8 RC_CHANNELS_FUNCTION_ACRO=9 +uint8 RC_CHANNELS_FUNCTION_FLAPS=10 +uint8 RC_CHANNELS_FUNCTION_AUX_1=11 +uint8 RC_CHANNELS_FUNCTION_AUX_2=12 +uint8 RC_CHANNELS_FUNCTION_AUX_3=13 +uint8 RC_CHANNELS_FUNCTION_AUX_4=14 +uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 +uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 +uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 +uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 +uint8 RC_CHANNELS_FUNCTION_TRANSITION=21 +uint8 RC_CHANNELS_FUNCTION_GEAR=22 +uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23 +uint8 RC_CHANNELS_FUNCTION_STAB=24 +uint8 RC_CHANNELS_FUNCTION_MAN=25 + +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[18] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[26] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout +uint32 frame_drop_count # Number of dropped frames diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg new file mode 100644 index 000000000000..4a5cb5cfd18c --- /dev/null +++ b/msg/rc_parameter_map.msg @@ -0,0 +1,10 @@ +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # initial value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value diff --git a/msg/ros/actuator_controls_0.msg b/msg/ros/actuator_controls_0.msg new file mode 100644 index 000000000000..4424f197f848 --- /dev/null +++ b/msg/ros/actuator_controls_0.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/ros/actuator_controls_virtual_mc.msg b/msg/ros/actuator_controls_virtual_mc.msg new file mode 100644 index 000000000000..4424f197f848 --- /dev/null +++ b/msg/ros/actuator_controls_virtual_mc.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/safety.msg b/msg/safety.msg new file mode 100644 index 000000000000..d69f12509e7d --- /dev/null +++ b/msg/safety.msg @@ -0,0 +1,4 @@ +bool safety_switch_available # Set to true if a safety switch is connected +bool safety_off # Set to true if safety is off +bool override_available # Set to true if external override system is connected +bool override_enabled # Set to true if override is engaged diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg new file mode 100644 index 000000000000..0efa030a87bf --- /dev/null +++ b/msg/satellite_info.msg @@ -0,0 +1,8 @@ +uint8 SAT_INFO_MAX_SATELLITES = 20 + +uint8 count # Number of satellites in satellite info +uint8[20] svid # Space vehicle ID [1..255], see scheme below +uint8[20] used # 0: Satellite not used, 1: used for navigation +uint8[20] elevation # Elevation (0: right on top of receiver, 90: on the horizon) of satellite +uint8[20] azimuth # Direction of satellite, 0: 0 deg, 255: 360 deg. +uint8[20] snr # dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg new file mode 100644 index 000000000000..d6d957cb8fd5 --- /dev/null +++ b/msg/sensor_accel.msg @@ -0,0 +1,18 @@ +uint64 integral_dt # integration time +uint64 error_count +float32 x # acceleration in the NED X board axis in m/s^2 +float32 y # acceleration in the NED Y board axis in m/s^2 +float32 z # acceleration in the NED Z board axis in m/s^2 +float32 x_integral # velocity in the NED X board axis in m/s over the integration time frame +float32 y_integral # velocity in the NED Y board axis in m/s over the integration time frame +float32 z_integral # velocity in the NED Z board axis in m/s over the integration time frame +float32 temperature # temperature in degrees celsius +float32 range_m_s2 # range in m/s^2 (+- this value) +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw + +uint32 device_id # unique device ID for the sensor that does not change between power cycles diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg new file mode 100644 index 000000000000..2d0d0528b758 --- /dev/null +++ b/msg/sensor_baro.msg @@ -0,0 +1,5 @@ +float32 pressure # static pressure measurement in millibar +float32 altitude # ISA pressure altitude in meters +float32 temperature # static temperature measurement in deg C +uint64 error_count +uint32 device_id # Sensor ID that must be unique for each baro sensor and must not change diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg new file mode 100644 index 000000000000..9197dc87bfff --- /dev/null +++ b/msg/sensor_bias.msg @@ -0,0 +1,30 @@ +# +# Sensor readings and in-run biases in SI-unit form. Sensor readings are compensated for static offsets, +# scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available). +# + +float32 gyro_x # Bias corrected angular velocity about X body axis (roll) (in rad/s) +float32 gyro_y # Bias corrected angular velocity about Y body axis (pitch) (in rad/s) +float32 gyro_z # Bias corrected angular velocity about Z body axis (yaw) (in rad/s) + +float32 accel_x # Bias corrected acceleration in body X axis (in rad/s) +float32 accel_y # Bias corrected acceleration in body Y axis (in rad/s) +float32 accel_z # Bias corrected acceleration in body Z axis (in rad/s) + +float32 mag_x # Bias corrected magnetometer reading in body X axis (in Gauss) +float32 mag_y # Bias corrected magnetometer reading in body Y axis (in Gauss) +float32 mag_z # Bias corrected magnetometer reading in body Z axis (in Gauss) + +# In-run bias estimates (subtract from uncorrected data) + +float32 gyro_x_bias # X gyroscope in-run bias in body frame (rad/s, x forward) +float32 gyro_y_bias # Y gyroscope in-run bias in body frame (rad/s, y right) +float32 gyro_z_bias # Z gyroscope in-run bias in body frame (rad/s, z down) + +float32 accel_x_bias # X accelerometer in-run bias in body frame (m/s^2, x forward) +float32 accel_y_bias # Y accelerometer in-run bias in body frame (m/s^2, y right) +float32 accel_z_bias # Z accelerometer in-run bias in body frame (m/s^2, z down) + +float32 mag_x_bias # X magnetometer in-run bias in body frame (Gauss, x forward) +float32 mag_y_bias # Y magnetometer in-run bias in body frame (Gauss, y right) +float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) \ No newline at end of file diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg new file mode 100644 index 000000000000..7b6fc7b51cc5 --- /dev/null +++ b/msg/sensor_combined.msg @@ -0,0 +1,24 @@ +# +# Sensor readings in SI-unit form. +# +# These fields are scaled and offset-compensated where possible and do not +# change with board revisions and sensor updates. +# + +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid + + +# gyro timstamp is equal to the timestamp of the message +float32[3] gyro_rad # average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in us + +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in us + +int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss + +int32 baro_timestamp_relative # timestamp + baro_timestamp_relative = Barometer timestamp +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg new file mode 100644 index 000000000000..2529a5324036 --- /dev/null +++ b/msg/sensor_correction.msg @@ -0,0 +1,55 @@ +# +# Sensor corrections in SI-unit form for the voted sensor +# + +# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +# corrections for uORB instance 0 +float32[3] gyro_offset_0 # gyro XYZ offsets in the sensor frame in rad/s +float32[3] gyro_scale_0 # gyro XYZ scale factors in the sensor frame + +# corrections for uORB instance 1 +float32[3] gyro_offset_1 # gyro XYZ offsets in the sensor frame in rad/s +float32[3] gyro_scale_1 # gyro XYZ scale factors in the sensor frame + +# corrections for uORB instance 2 +float32[3] gyro_offset_2 # gyro XYZ offsets in the sensor frame in rad/s +float32[3] gyro_scale_2 # gyro XYZ scale factors in the sensor frame + +# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +# corrections for uORB instance 0 +float32[3] accel_offset_0 # accelerometer XYZ offsets in the sensor frame in m/s/s +float32[3] accel_scale_0 # accelerometer XYZ scale factors in the sensor frame + +# corrections for uORB instance 1 +float32[3] accel_offset_1 # accelerometer XYZ offsets in the sensor frame in m/s/s +float32[3] accel_scale_1 # accelerometer XYZ scale factors in the sensor frame + +# corrections for uORB instance 2 +float32[3] accel_offset_2 # accelerometer XYZ offsets in the sensor frame in m/s/s +float32[3] accel_scale_2 # accelerometer XYZ scale factors in the sensor frame + +# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +# corrections for uORB instance 0 +float32 baro_offset_0 # barometric pressure offsets in the sensor frame in m/s/s +float32 baro_scale_0 # barometric pressure scale factors in the sensor frame + +# corrections for uORB instance 1 +float32 baro_offset_1 # barometric pressure offsets in the sensor frame in m/s/s +float32 baro_scale_1 # barometric pressure scale factors in the sensor frame + +# corrections for uORB instance 2 +float32 baro_offset_2 # barometric pressure offsets in the sensor frame in m/s/s +float32 baro_scale_2 # barometric pressure scale factors in the sensor frame + +uint8 selected_gyro_instance # gyro uORB topic instance for the voted sensor +uint8 selected_accel_instance # accelerometer uORB topic instance for the voted sensor +uint8 selected_baro_instance # barometric pressure uORB topic instance for the voted sensor + +# Mapping from uORB index to parameter index for each sensor type. For example accel_mapping[1] contains the +# compensation parameter system index for the sensor_accel uORB index 1 data. +uint8[3] gyro_mapping +uint8[3] accel_mapping +uint8[3] baro_mapping diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg new file mode 100644 index 000000000000..8b0b72f12d4d --- /dev/null +++ b/msg/sensor_gyro.msg @@ -0,0 +1,18 @@ +uint64 integral_dt # integration time +uint64 error_count +float32 x # angular velocity in the NED X board axis in rad/s +float32 y # angular velocity in the NED Y board axis in rad/s +float32 z # angular velocity in the NED Z board axis in rad/s +float32 x_integral # delta angle in the NED X board axis in rad/s in the integration time frame +float32 y_integral # delta angle in the NED Y board axis in rad/s in the integration time frame +float32 z_integral # delta angle in the NED Z board axis in rad/s in the integration time frame +float32 temperature # temperature in degrees celcius +float32 range_rad_s +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw + +uint32 device_id # unique device ID for the sensor that does not change between power cycles diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg new file mode 100644 index 000000000000..2eeeca3608c5 --- /dev/null +++ b/msg/sensor_mag.msg @@ -0,0 +1,15 @@ +uint64 error_count +float32 x +float32 y +float32 z +float32 range_ga +float32 scaling +float32 temperature + +int16 x_raw +int16 y_raw +int16 z_raw + +uint32 device_id # unique device ID for the sensor that does not change between power cycles +bool is_external # if true the mag is external (i.e. not built into the board) + diff --git a/msg/sensor_preflight.msg b/msg/sensor_preflight.msg new file mode 100644 index 000000000000..293953f886bf --- /dev/null +++ b/msg/sensor_preflight.msg @@ -0,0 +1,7 @@ +# +# Pre-flight sensor check metrics. These will be zero if the vehicle only has one sensor. +# The topic will not be updated when the vehicle is armed +# +float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). +float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). +float32 mag_inconsistency_ga # magnitude of maximum difference between Mag instances in (Gauss). diff --git a/msg/sensor_selection.msg b/msg/sensor_selection.msg new file mode 100644 index 000000000000..c6141f069002 --- /dev/null +++ b/msg/sensor_selection.msg @@ -0,0 +1,9 @@ +# +# Sensor ID's for the voted sensors output on the sensor_combined topic. +# Will be updated on startup of the sensor module and when sensor selection changes +# + +uint32 accel_device_id # unique device ID for the selected accelerometers +uint32 baro_device_id # unique device ID for the selected barometer +uint32 gyro_device_id # unique device ID for the selected rate gyros +uint32 mag_device_id # unique device ID for the selected magnetometer diff --git a/msg/servorail_status.msg b/msg/servorail_status.msg new file mode 100644 index 000000000000..39edf80e4f5a --- /dev/null +++ b/msg/servorail_status.msg @@ -0,0 +1,2 @@ +float32 voltage_v # Servo rail voltage in volts +float32 rssi_v # RSSI pin voltage in volts diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg new file mode 100644 index 000000000000..bd0b9dd841de --- /dev/null +++ b/msg/subsystem_info.msg @@ -0,0 +1,22 @@ +uint64 SUBSYSTEM_TYPE_GYRO = 1 +uint64 SUBSYSTEM_TYPE_ACC = 2 +uint64 SUBSYSTEM_TYPE_MAG = 4 +uint64 SUBSYSTEM_TYPE_ABSPRESSURE = 8 +uint64 SUBSYSTEM_TYPE_DIFFPRESSURE = 16 +uint64 SUBSYSTEM_TYPE_GPS = 32 +uint64 SUBSYSTEM_TYPE_OPTICALFLOW = 64 +uint64 SUBSYSTEM_TYPE_CVPOSITION = 128 +uint64 SUBSYSTEM_TYPE_LASERPOSITION = 256 +uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512 +uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024 +uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048 +uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096 +uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384 +uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 32768 +uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 65536 +uint64 SUBSYSTEM_TYPE_RANGEFINDER = 131072 + +bool present +bool enabled +bool ok +uint64 subsystem_type diff --git a/msg/system_power.msg b/msg/system_power.msg new file mode 100644 index 000000000000..13730f1ce62d --- /dev/null +++ b/msg/system_power.msg @@ -0,0 +1,19 @@ +float32 voltage5V_v # peripheral 5V rail voltage +float32 voltage3V3_v # Sensor 3V3 rail voltage +uint8 v3v3_valid # Sensor 3V3 rail voltage was read. +uint8 usb_connected # USB is connected when 1 +uint8 brick_valid # brick bits power is good when bit 1 +uint8 usb_valid # USB is valid when 1 +uint8 servo_valid # servo power is good when 1 +uint8 periph_5V_OC # peripheral overcurrent when 1 +uint8 hipower_5V_OC # hi power peripheral overcurrent when 1 + +uint8 BRICK1_VALID_SHIFTS=0 +uint8 BRICK1_VALID_MASK=1 +uint8 BRICK2_VALID_SHIFTS=1 +uint8 BRICK2_VALID_MASK=2 +uint8 BRICK3_VALID_SHIFTS=2 +uint8 BRICK3_VALID_MASK=4 +uint8 BRICK4_VALID_SHIFTS=3 +uint8 BRICK4_VALID_MASK=8 + diff --git a/msg/task_stack_info.msg b/msg/task_stack_info.msg new file mode 100644 index 000000000000..1dea74211ec0 --- /dev/null +++ b/msg/task_stack_info.msg @@ -0,0 +1,6 @@ +# stack information for a single running process + +uint8 MAX_REPORT_TASK_NAME_LEN = 16 + +uint16 stack_free +uint8[16] task_name diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg new file mode 100644 index 000000000000..45d1bd005de3 --- /dev/null +++ b/msg/tecs_status.msg @@ -0,0 +1,27 @@ +uint8 TECS_MODE_NORMAL = 0 +uint8 TECS_MODE_UNDERSPEED = 1 +uint8 TECS_MODE_TAKEOFF = 2 +uint8 TECS_MODE_LAND = 3 +uint8 TECS_MODE_LAND_THROTTLELIM = 4 +uint8 TECS_MODE_BAD_DESCENT = 5 +uint8 TECS_MODE_CLIMBOUT = 6 + + +float32 altitudeSp +float32 altitude_filtered +float32 flightPathAngleSp +float32 flightPathAngle +float32 airspeedSp +float32 airspeed_filtered +float32 airspeedDerivativeSp +float32 airspeedDerivative + +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_integ +float32 pitch_integ + +uint8 mode diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg new file mode 100644 index 000000000000..7232102410d1 --- /dev/null +++ b/msg/telemetry_status.msg @@ -0,0 +1,19 @@ +uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0 +uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1 +uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 +uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 +uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 +uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5 + +uint64 heartbeat_time # Time of last received heartbeat from remote system +uint64 telem_time # Time of last received telemetry status packet, 0 for none +uint8 type # type of the radio hardware +uint8 rssi # local signal strength +uint8 remote_rssi # remote signal strength +uint16 rxerrors # receive errors +uint16 fixed # count of error corrected packets +uint8 noise # background noise level +uint8 remote_noise # remote background noise level +uint8 txbuf # how full the tx buffer is as a percentage +uint8 system_id # system id of the remote system +uint8 component_id # component id of the remote system diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template new file mode 100644 index 000000000000..76abab163db9 --- /dev/null +++ b/msg/templates/px4/ros/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include "px4/@(topic_name).h" +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return "@(topic_name)";} +}; + +}; diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template new file mode 100644 index 000000000000..1707dfc0152a --- /dev/null +++ b/msg/templates/px4/uorb/msg.h.template @@ -0,0 +1,100 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +native_type = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ +@[for multi_topic in topics]@ +@{ +cpp_class = 'px4_%s'%multi_topic +}@ + + +class __EXPORT @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));} +}; +@[end for] + +}; diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template new file mode 100644 index 000000000000..3437e5c109cd --- /dev/null +++ b/msg/templates/uorb/msg.cpp.template @@ -0,0 +1,75 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name + +sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) +struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) +topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] +}@ + +#include + +@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" +@# This is used for the logger +constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; + +@[for multi_topic in topics]@ +ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields); +@[end for] diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template new file mode 100644 index 000000000000..1d1c74271ed6 --- /dev/null +++ b/msg/templates/uorb/msg.h.template @@ -0,0 +1,135 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## + +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include '%(name)) +}@ + +@# Constants c style +#ifndef __cplusplus +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] +#endif + +@############################## +@# Main struct of message +@############################## +@{ + +def print_parsed_fields(): + # sort fields (using a stable sort) + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) + # loop over all fields and print the type and name + for field in sorted_fields: + if (not field.is_header): + print_field_def(field) +}@ + +#ifdef __cplusplus +@#class @(uorb_struct) { +struct __EXPORT @(uorb_struct) { +@#public: +#else +struct @(uorb_struct) { +#endif +@# timestamp at the beginning of each topic is required for logger + uint64_t timestamp; // required for logger +@print_parsed_fields() + +#ifdef __cplusplus +@# Constants again c++-ified +@{ +for constant in spec.constants: + type_name = constant.type + if type_name in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type_name] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type_name)) + + print('\tstatic constexpr %s %s = %s;'%(type_px4, constant.name, int(constant.val))) +} +#endif +}; + +/* register this as object request broker structure */ +@[for multi_topic in topics]@ +ORB_DECLARE(@multi_topic); +@[end for] diff --git a/msg/templates/uorb/uORBTopics.cpp.template b/msg/templates/uorb/uORBTopics.cpp.template new file mode 100644 index 000000000000..c8e29cd18972 --- /dev/null +++ b/msg/templates/uorb/uORBTopics.cpp.template @@ -0,0 +1,73 @@ +@############################################### +@# +@# EmPy template for generating uORBTopics.cpp file +@# for logging purposes +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +@{ +msg_names = [mn.replace(".msg", "") for mn in msgs] +msgs_count = len(msg_names) +msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates +msgs_count_all = len(msg_names_all) +}@ +@[for msg_name in msg_names]@ +#include +@[end for] + +const size_t _uorb_topics_count = @(msgs_count_all); +const struct orb_metadata* _uorb_topics_list[_uorb_topics_count] = { +@[for idx, msg_name in enumerate(msg_names_all, 1)]@ + ORB_ID(@(msg_name))@[if idx != msgs_count_all],@[end if] +@[end for] +}; + +size_t orb_topics_count() +{ + return _uorb_topics_count; +} + +const struct orb_metadata **orb_get_topics() +{ + return _uorb_topics_list; +} diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template new file mode 100644 index 000000000000..2801a7bcee1e --- /dev/null +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template @@ -0,0 +1,225 @@ +@############################################### +@# +@# EmPy template for generating microRTPS_client.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ +from px_generate_uorb_topic_files import MsgScope # this is in Tools/ +from uorb_rtps_message_ids import * # this is in Tools/ + +topic_names = [single_spec.short_name for single_spec in spec] +send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] + +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "microRTPS_transport.h" +#include "microRTPS_client.h" + +#include +#include +#include +#include + +#include +#include +#include + +@[for topic in list(set(topic_names))]@ +#include +#include +@[end for]@ + +void* send(void *data); + +@[if send_topics]@ +void* send(void* /*unused*/) +{ + char data_buffer[BUFFER_SIZE] = {}; + uint64_t sent = 0, total_sent = 0; + int loop = 0, read = 0; + uint32_t length = 0; + uint16_t header_length = 0; + + /* subscribe to topics */ + int fds[@(len(send_topics))] = {}; + + // orb_set_interval statblish an update interval period in milliseconds. +@[for idx, topic in enumerate(send_topics)]@ + fds[@(idx)] = orb_subscribe(ORB_ID(@(topic))); + orb_set_interval(fds[@(idx)], _options.update_time_ms); +@[end for]@ + + // microBuffer to serialized using the user defined buffer + struct microBuffer microBufferWriter; + header_length=transport_node->get_header_length(); + initStaticAlignedBuffer(&data_buffer[header_length], BUFFER_SIZE-header_length, µBufferWriter); + // microCDR structs for managing the microBuffer + struct microCDR microCDRWriter; + initMicroCDR(µCDRWriter, µBufferWriter); + + struct timespec begin; + px4_clock_gettime(CLOCK_REALTIME, &begin); + + while (!_should_exit_task) + { + bool updated; +@[for idx, topic in enumerate(send_topics)]@ + orb_check(fds[@(idx)], &updated); + if (updated) + { + // obtained data for the file descriptor + struct @(topic)_s data; + // copy raw data into local buffer + if (orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data) == 0) { + /* payload is shifted by header length to make room for header*/ + serialize_@(topic)(&data, &data_buffer[header_length], &length, µCDRWriter); + + if (0 < (read = transport_node->write((char)@(message_id(topic)), data_buffer, length))) + { + total_sent += read; + ++sent; + } + } + } +@[end for]@ + + usleep(_options.sleep_ms*1000); + ++loop; + } + + struct timespec end; + px4_clock_gettime(CLOCK_REALTIME, &end); + double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); + printf("\nSENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s\n", + sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs)); + + return nullptr; +} + +static int launch_send_thread(pthread_t &sender_thread) +{ + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(4000)); + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_DEFAULT; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + pthread_create(&sender_thread, &sender_thread_attr, send, nullptr); + pthread_attr_destroy(&sender_thread_attr); + + return 0; +} +@[end if]@ + +void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop) +{ +@[if recv_topics]@ + + char data_buffer[BUFFER_SIZE] = {}; + int read = 0; + uint8_t topic_ID = 255; + + // Declare received topics +@[for topic in recv_topics]@ + struct @(topic)_s @(topic)_data; + orb_advert_t @(topic)_pub = nullptr; +@[end for]@ + + // microBuffer to deserialized using the user defined buffer + struct microBuffer microBufferReader; + initDeserializedAlignedBuffer(data_buffer, BUFFER_SIZE, µBufferReader); + // microCDR structs for managing the microBuffer + struct microCDR microCDRReader; + initMicroCDR(µCDRReader, µBufferReader); +@[end if]@ + + px4_clock_gettime(CLOCK_REALTIME, &begin); + _should_exit_task = false; +@[if send_topics]@ + + // create a thread for sending data to the simulator + pthread_t sender_thread; + launch_send_thread(sender_thread); +@[end if]@ + + while (!_should_exit_task) + { +@[if recv_topics]@ + while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) + { + total_read += read; + switch (topic_ID) + { +@[for topic in recv_topics]@ + case @(message_id(topic)): + { + deserialize_@(topic)(&@(topic)_data, data_buffer, µCDRReader); + if (!@(topic)_pub) { + @(topic)_pub = orb_advertise(ORB_ID(@(topic)), &@(topic)_data); + } else { + orb_publish(ORB_ID(@(topic)), @(topic)_pub, &@(topic)_data); + } + ++received; + } + break; +@[end for]@ + default: + printf("Unexpected topic ID\n"); + break; + } + } +@[end if]@ + + // loop forever if informed loop number is negative + if (_options.loops >= 0 && loop >= _options.loops) break; + + usleep(_options.sleep_ms*1000); + ++loop; + } +@[if send_topics]@ + _should_exit_task = true; + pthread_join(sender_thread, nullptr); +@[end if]@ +} diff --git a/msg/templates/uorb_microcdr/msg.cpp.template b/msg/templates/uorb_microcdr/msg.cpp.template new file mode 100644 index 000000000000..6b4320372620 --- /dev/null +++ b/msg/templates/uorb_microcdr/msg.cpp.template @@ -0,0 +1,164 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#include +#include +#include +#include + +@################################################# +@# Searching for serialize function per each field +@################################################# +@{ + +def print_info(field): + print("type: ", field.type, "name: ", field.name, "base_type: ", \ + field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \ + "is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header]) + +def print_level_info(fields): + for field in fields: + print_info(field) + if (not field.is_builtin): + print("\n") + children_fields = get_children_fields(field.base_type, search_path) + print_level_info(children_fields) + print("\n") + +def walk_through_parsed_fields(): + print_level_info(spec.parsed_fields()) + +def get_serialization_type_name(type_name): + if type_name in type_serialize_map: + return type_serialize_map[type_name] + else: + raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name)) + +def add_serialize_functions(fields, scope_name): + for field in fields: + if (not field.is_header): + if (field.is_builtin): + if (not field.is_array): + print(" serialize"+str(get_serialization_type_name(field.type))+"(input->"+scope_name+str(field.name)+", microCDRWriter);") + else: + print(" serialize"+str(get_serialization_type_name(field.base_type))+"Array(input->"+scope_name+str(field.name)+", "+str(field.array_len)+", microCDRWriter);") + else: + name = field.name + children_fields = get_children_fields(field.base_type, search_path) + if (scope_name): name = scope_name + name + if (not field.is_array): + add_serialize_functions(children_fields, name + '.') + else: + for i in range(field.array_len): + add_serialize_functions(children_fields, name + ('[%d].' %i)) + +def add_deserialize_functions(fields, scope_name): + for field in fields: + if (not field.is_header): + if (field.is_builtin): + if (not field.is_array): + print(" deserialize"+str(get_serialization_type_name(field.type))+"(&output->"+scope_name+str(field.name)+", microCDRReader);") + else: + for i in range(field.array_len): + print(" deserialize"+str(get_serialization_type_name(field.base_type))+"(&output->"+scope_name+str(field.name)+ str('[%d]' %i) +", microCDRReader);") + else: + name = field.name + children_fields = get_children_fields(field.base_type, search_path) + if (scope_name): name = scope_name + name + if (not field.is_array): + add_deserialize_functions(children_fields, name + '.') + else: + for i in range(field.array_len): + add_deserialize_functions(children_fields, name + ('[%d].' %i)) + +def add_code_to_serialize(): + # sort fields (using a stable sort) as in the declaration of the type + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + add_serialize_functions(sorted_fields, "") + +def add_code_to_deserialize(): + # sort fields (using a stable sort) as in the declaration of the type + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + add_deserialize_functions(sorted_fields, "") +}@ + +void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter) +{ + if (nullptr == input || nullptr == output || nullptr == length || nullptr == microCDRWriter) return; + + resetStaticMicroCDRForSerialize(microCDRWriter); + + serializeUnsignedLong(input->timestamp, microCDRWriter); +@add_code_to_serialize() + + (*length) = microCDRWriter->m_microBuffer->m_serializedBuffer; +} + +void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader) +{ + if (nullptr == output || nullptr == input || nullptr == microCDRReader) return; + + resetStaticMicroCDRForDeserialize(microCDRReader); + + deserializeUnsignedLong(&output->timestamp, microCDRReader); +@add_code_to_deserialize() +} \ No newline at end of file diff --git a/msg/templates/uorb_microcdr/msg.h.template b/msg/templates/uorb_microcdr/msg.h.template new file mode 100644 index 000000000000..ec05f4ead0bb --- /dev/null +++ b/msg/templates/uorb_microcdr/msg.h.template @@ -0,0 +1,75 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include + +struct microCDR; + + +void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter); +void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader); diff --git a/src/modules/vtol_att_control/kitepower_params.c b/msg/templates/uorb_microcdr/uORBTopics.cpp.template similarity index 78% rename from src/modules/vtol_att_control/kitepower_params.c rename to msg/templates/uorb_microcdr/uORBTopics.cpp.template index 2f2a0138082a..bc245b5f5442 100644 --- a/src/modules/vtol_att_control/kitepower_params.c +++ b/msg/templates/uorb_microcdr/uORBTopics.cpp.template @@ -1,6 +1,18 @@ +@############################################### +@# +@# EmPy template for generating uORBTopics.cpp file +@# for logging purposes +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,13 +43,4 @@ * ****************************************************************************/ -/** - * @file kitepower_params.c - * Parameters for vtol attitude controller. - * - * @author Gabriel König - */ - -#include - -//empty at the moment +#include \ No newline at end of file diff --git a/msg/templates/urtps/Publisher.cpp.template b/msg/templates/urtps/Publisher.cpp.template new file mode 100644 index 000000000000..77125c08dd9c --- /dev/null +++ b/msg/templates/urtps/Publisher.cpp.template @@ -0,0 +1,154 @@ +@############################################### +@# +@# EmPy template for generating _uRTPS_UART.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +topic = spec.short_name +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/*! + * @@file @(topic)_Publisher.cpp + * This file contains the implementation of the publisher functions. + * + * This file was generated by the tool fastcdrgen. + */ + +#include +#include +#include +#include + +#include + +#include + +#include "@(topic)_Publisher.h" + + +@(topic)_Publisher::@(topic)_Publisher() : mp_participant(nullptr), mp_publisher(nullptr) {} + +@(topic)_Publisher::~@(topic)_Publisher() { Domain::removeParticipant(mp_participant);} + +bool @(topic)_Publisher::init() +{ + // Create RTPSParticipant + + ParticipantAttributes PParam; + PParam.rtps.builtin.domainId = 0; + PParam.rtps.builtin.leaseDuration = c_TimeInfinite; + PParam.rtps.setName("Participant_publisher"); //You can put here the name you want + mp_participant = Domain::createParticipant(PParam); + if(mp_participant == nullptr) + return false; + + //Register the type + + Domain::registerType(mp_participant,(TopicDataType*) &myType); + + // Create Publisher + + PublisherAttributes Wparam; + Wparam.topic.topicKind = NO_KEY; + Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered + Wparam.topic.topicName = "@(topic)_PubSubTopic"; + mp_publisher = Domain::createPublisher(mp_participant,Wparam,(PublisherListener*)&m_listener); + if(mp_publisher == nullptr) + return false; + //std::cout << "Publisher created, waiting for Subscribers." << std::endl; + return true; +} + +void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub,MatchingInfo& info) +{ + if (info.status == MATCHED_MATCHING) + { + n_matched++; + std::cout << "Publisher matched" << std::endl; + } + else + { + n_matched--; + std::cout << "Publisher unmatched" << std::endl; + } +} + +void @(topic)_Publisher::run() +{ + while(m_listener.n_matched == 0) + { + eClock::my_sleep(250); // Sleep 250 ms + } + + // Publication code + + @(topic)_ st; + + /* Initialize your structure here */ + + int msgsent = 0; + char ch = 'y'; + do + { + if(ch == 'y') + { + mp_publisher->write(&st); ++msgsent; + std::cout << "Sending sample, count=" << msgsent << ", send another sample?(y-yes,n-stop): "; + } + else if(ch == 'n') + { + std::cout << "Stopping execution " << std::endl; + break; + } + else + { + std::cout << "Command " << ch << " not recognized, please enter \"y/n\":"; + } + }while(std::cin >> ch); +} + +void @(topic)_Publisher::publish(@(topic)_* st) +{ + mp_publisher->write(st); +} diff --git a/msg/templates/urtps/Publisher.h.template b/msg/templates/urtps/Publisher.h.template new file mode 100644 index 000000000000..6776562442e9 --- /dev/null +++ b/msg/templates/urtps/Publisher.h.template @@ -0,0 +1,92 @@ +@############################################### +@# +@# EmPy template for generating _uRTPS_UART.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +topic = spec.short_name +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/*! + * @@file @(topic)_Publisher.h + * This header file contains the declaration of the publisher functions. + * + * This file was generated by the tool fastcdrgen. + */ + + +#ifndef _@(topic)__PUBLISHER_H_ +#define _@(topic)__PUBLISHER_H_ + +#include +#include + +#include "@(topic)_PubSubTypes.h" + +using namespace eprosima::fastrtps; + +class @(topic)_Publisher +{ +public: + @(topic)_Publisher(); + virtual ~@(topic)_Publisher(); + bool init(); + void run(); + void publish(@(topic)_* st); +private: + Participant *mp_participant; + Publisher *mp_publisher; + + class PubListener : public PublisherListener + { + public: + PubListener() : n_matched(0){}; + ~PubListener(){}; + void onPublicationMatched(Publisher* pub,MatchingInfo& info); + int n_matched; + } m_listener; + @(topic)_PubSubType myType; +}; + +#endif // _@(topic)__PUBLISHER_H_ \ No newline at end of file diff --git a/msg/templates/urtps/RtpsTopics.cpp.template b/msg/templates/urtps/RtpsTopics.cpp.template new file mode 100644 index 000000000000..6f031a10c3e1 --- /dev/null +++ b/msg/templates/urtps/RtpsTopics.cpp.template @@ -0,0 +1,159 @@ +@############################################### +@# +@# EmPy template for generating RtpsTopics.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ +from px_generate_uorb_topic_files import MsgScope # this is in Tools/ +from uorb_rtps_message_ids import * # this is in Tools/ + +topic_names = [single_spec.short_name for single_spec in spec] +send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RtpsTopics.h" + +bool RtpsTopics::init() +{ +@[if recv_topics]@ + // Initialise subscribers +@[for topic in recv_topics]@ + if (_@(topic)_sub.init()) { + std::cout << "@(topic) subscriber started" << std::endl; + } else { + std::cout << "ERROR starting @(topic) subscriber" << std::endl; + return false; + } + +@[end for]@ +@[end if]@ +@[if send_topics]@ + // Initialise publishers +@[for topic in send_topics]@ + if (_@(topic)_pub.init()) { + std::cout << "@(topic) publisher started" << std::endl; + } else { + std::cout << "ERROR starting @(topic) publisher" << std::endl; + return false; + } + +@[end for]@ +@[end if]@ + return true; +} + +@[if send_topics]@ +void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len) +{ + switch (topic_ID) + { +@[for topic in send_topics]@ + case @(message_id(topic)): // @(topic) + { + @(topic)_ st; + eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); + eprosima::fastcdr::Cdr cdr_des(cdrbuffer); + st.deserialize(cdr_des); + _@(topic)_pub.publish(&st); + } + break; +@[end for]@ + default: + printf("Unexpected topic ID to publish\n"); + break; + } +} +@[end if]@ +@[if recv_topics]@ + +bool RtpsTopics::hasMsg(uint8_t *topic_ID) +{ + if (nullptr == topic_ID) return false; + + *topic_ID = 0; + while (_next_sub_idx < @(len(recv_topics)) && 0 == *topic_ID) + { + switch (_sub_topics[_next_sub_idx]) + { +@[for topic in recv_topics]@ + case @(message_id(topic)): if (_@(topic)_sub.hasMsg()) *topic_ID = @(message_id(topic)); break; +@[end for]@ + default: + printf("Unexpected topic ID to check hasMsg\n"); + break; + } + _next_sub_idx++; + } + + if (0 == *topic_ID) + { + _next_sub_idx = 0; + return false; + } + + return true; +} + +bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) +{ + bool ret = false; + switch (topic_ID) + { +@[for topic in recv_topics]@ + case @(message_id(topic)): // @(topic) + if (_@(topic)_sub.hasMsg()) + { + @(topic)_ msg = _@(topic)_sub.getMsg(); + msg.serialize(scdr); + ret = true; + } + break; +@[end for]@ + default: + printf("Unexpected topic ID '%hhu' to getMsg\n", topic_ID); + break; + } + + return ret; +} +@[end if]@ diff --git a/msg/templates/urtps/RtpsTopics.h.template b/msg/templates/urtps/RtpsTopics.h.template new file mode 100644 index 000000000000..07dce5e6f7ba --- /dev/null +++ b/msg/templates/urtps/RtpsTopics.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# EmPy template for generating RtpsTopics.h file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ +from px_generate_uorb_topic_files import MsgScope # this is in Tools/ +from uorb_rtps_message_ids import * # this is in Tools/ + +topic_names = [single_spec.short_name for single_spec in spec] +send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +@[for topic in send_topics]@ +#include "@(topic)_Publisher.h" +@[end for]@ +@[for topic in recv_topics]@ +#include "@(topic)_Subscriber.h" +@[end for]@ + +class RtpsTopics { +public: + bool init(); +@[if send_topics]@ + void publish(uint8_t topic_ID, char data_buffer[], size_t len); +@[end if]@ +@[if recv_topics]@ + bool hasMsg(uint8_t *topic_ID); + bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr); +@[end if]@ + +private: +@[if send_topics]@ + // Publishers +@[for topic in send_topics]@ + @(topic)_Publisher _@(topic)_pub; +@[end for]@ +@[end if]@ + +@[if recv_topics]@ + // Subscribers +@[for topic in recv_topics]@ + @(topic)_Subscriber _@(topic)_sub; +@[end for]@ + + unsigned _next_sub_idx = 0; + char _sub_topics[@(len(recv_topics))] = { +@[for topic in recv_topics]@ + @(message_id(topic)), // @(topic) +@[end for]@ + }; +@[end if]@ +}; diff --git a/msg/templates/urtps/Subscriber.cpp.template b/msg/templates/urtps/Subscriber.cpp.template new file mode 100644 index 000000000000..686943d93f57 --- /dev/null +++ b/msg/templates/urtps/Subscriber.cpp.template @@ -0,0 +1,146 @@ +@############################################### +@# +@# EmPy template for generating _uRTPS_UART.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +topic = spec.short_name +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/*! + * @@file @(topic)_Subscriber.cpp + * This file contains the implementation of the subscriber functions. + * + * This file was generated by the tool fastcdrgen. + */ + +#include +#include +#include +#include + +#include + +#include "@(topic)_Subscriber.h" + + +@(topic)_Subscriber::@(topic)_Subscriber() : mp_participant(nullptr), mp_subscriber(nullptr) {} + +@(topic)_Subscriber::~@(topic)_Subscriber() { Domain::removeParticipant(mp_participant);} + +bool @(topic)_Subscriber::init() +{ + // Create RTPSParticipant + + ParticipantAttributes PParam; + PParam.rtps.builtin.domainId = 0; //MUST BE THE SAME AS IN THE PUBLISHER + PParam.rtps.builtin.leaseDuration = c_TimeInfinite; + PParam.rtps.setName("Participant_subscriber"); //You can put the name you want + mp_participant = Domain::createParticipant(PParam); + if(mp_participant == nullptr) + return false; + + //Register the type + + Domain::registerType(mp_participant,(TopicDataType*) &myType); + + // Create Subscriber + + SubscriberAttributes Rparam; + Rparam.topic.topicKind = NO_KEY; + Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber + Rparam.topic.topicName = "@(topic)_PubSubTopic"; + mp_subscriber = Domain::createSubscriber(mp_participant,Rparam,(SubscriberListener*)&m_listener); + if(mp_subscriber == nullptr) + return false; + return true; +} + +void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub,MatchingInfo& info) +{ + if (info.status == MATCHED_MATCHING) + { + n_matched++; + std::cout << "Subscriber matched" << std::endl; + } + else + { + n_matched--; + std::cout << "Subscriber unmatched" << std::endl; + } +} + +void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub) +{ + // Take data + if(sub->takeNextData(&msg, &m_info)) + { + if(m_info.sampleKind == ALIVE) + { + // Print your structure data here. + ++n_msg; + //std::cout << "Sample received, count=" << n_msg << std::endl; + has_msg = true; + + } + } +} + +void @(topic)_Subscriber::run() +{ + std::cout << "Waiting for Data, press Enter to stop the Subscriber. "<_uRTPS_UART.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ + +topic = spec.short_name +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/*! + * @@file @(topic)_Subscriber.h + * This header file contains the declaration of the subscriber functions. + * + * This file was generated by the tool fastcdrgen. + */ + + +#ifndef _@(topic)__SUBSCRIBER_H_ +#define _@(topic)__SUBSCRIBER_H_ + +#include +#include +#include +#include "@(topic)_PubSubTypes.h" + +using namespace eprosima::fastrtps; + +class @(topic)_Subscriber +{ +public: + @(topic)_Subscriber(); + virtual ~@(topic)_Subscriber(); + bool init(); + void run(); + bool hasMsg(); + @(topic)_ getMsg(); +private: + Participant *mp_participant; + Subscriber *mp_subscriber; + + class SubListener : public SubscriberListener + { + public: + SubListener() : n_matched(0),n_msg(0){}; + ~SubListener(){}; + void onSubscriptionMatched(Subscriber* sub,MatchingInfo& info); + void onNewDataMessage(Subscriber* sub); + SampleInfo_t m_info; + int n_matched; + int n_msg; + @(topic)_ msg; + bool has_msg = false; + + } m_listener; + @(topic)_PubSubType myType; +}; + +#endif // _@(topic)__SUBSCRIBER_H_ \ No newline at end of file diff --git a/msg/templates/urtps/microRTPS_agent.cpp.template b/msg/templates/urtps/microRTPS_agent.cpp.template new file mode 100644 index 000000000000..54f7316c8570 --- /dev/null +++ b/msg/templates/urtps/microRTPS_agent.cpp.template @@ -0,0 +1,310 @@ +@############################################### +@# +@# EmPy template for generating microRTPS_agent.cpp file +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@############################################### +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ +from px_generate_uorb_topic_files import MsgScope # this is in Tools/ +from uorb_rtps_message_ids import * # this is in Tools/ + +topic_names = [single_spec.short_name for single_spec in spec] +send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +}@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "microRTPS_transport.h" +#include "RtpsTopics.h" + +#define BUFFER_SIZE 1024 + +// Default values +#define DEVICE "/dev/ttyACM0" +#define SLEEP_US 1 +#define BAUDRATE B460800 +#define BAUDRATE_VAL 460800 +#define POLL_MS 0 +#define WAIT_CNST 2 +#define DEFAULT_RECV_PORT 2020 +#define DEFAULT_SEND_PORT 2019 + +using namespace eprosima; +using namespace eprosima::fastrtps; + +volatile sig_atomic_t running = 1; +Transport_node *transport_node = nullptr; +RtpsTopics topics; +uint32_t total_sent = 0, sent = 0; + +struct baudtype { + speed_t code; + uint32_t val; +}; + +const baudtype baudlist[] = { + [0] = {.code = B0, .val = 0}, + [1] = {.code = B9600, .val = 9600}, + [2] = {.code = B19200, .val = 19200}, + [3] = {.code = B38400, .val = 38400}, + [4] = {.code = B57600, .val = 57600}, + [5] = {.code = B115200, .val = 115200}, + [6] = {.code = B230400, .val = 230400}, + [7] = {.code = B460800, .val = 460800}, + [8] = {.code = B921600, .val = 921600}, +}; + +struct options { + enum class eTransports + { + UART, + UDP + }; + eTransports transport = options::eTransports::UART; + char device[64] = DEVICE; + int sleep_us = SLEEP_US; + baudtype baudrate = {.code=BAUDRATE,.val=BAUDRATE_VAL}; + int poll_ms = POLL_MS; + uint16_t recv_port = DEFAULT_RECV_PORT; + uint16_t send_port = DEFAULT_SEND_PORT; +} _options; + +static void usage(const char *name) +{ + printf("usage: %s [options]\n\n" + " -t [UART|UDP] Default UART\n" + " -d UART device. Default /dev/ttyACM0\n" + " -w Time in us for which each iteration sleep. Default 1ms\n" + " -b UART device baudrate. Default 460800\n" + " -p Time in ms to poll over UART. Default 1ms\n" + " -r UDP port for receiving. Default 2019\n" + " -s UDP port for sending. Default 2020\n", + name); +} + +baudtype getbaudrate(char *valstr) +{ + uint32_t baudval = strtoul(valstr, nullptr, 10); + for (unsigned int i=1; iclose(); +} +@[if recv_topics]@ + +void t_send(void *data) +{ + char data_buffer[BUFFER_SIZE] = {}; + int length = 0; + uint8_t topic_ID = 255; + + while (running) + { + // Send subscribed topics over UART + while (topics.hasMsg(&topic_ID)) + { + uint16_t header_length = get_header_length(); + /* make room for the header to fill in later */ + eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length); + eprosima::fastcdr::Cdr scdr(cdrbuffer); + if (topics.getMsg(topic_ID, scdr)) + { + length = scdr.getSerializedDataLength(); + if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) + { + total_sent += length; + ++sent; + } + } + } + + usleep(_options.sleep_us); + } +} +@[end if]@ + +int main(int argc, char** argv) +{ + if (-1 == parse_options(argc, argv)) + { + printf("EXITING...\n"); + return -1; + } + + // register signal SIGINT and signal handler + signal(SIGINT, signal_handler); + + switch (_options.transport) + { + case options::eTransports::UART: + { + transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms); + printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n", + _options.device, _options.baudrate.val, _options.sleep_us, _options.poll_ms); + } + break; + case options::eTransports::UDP: + { + transport_node = new UDP_node(_options.recv_port, _options.send_port); + printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dus\n\n", + _options.recv_port, _options.send_port, _options.sleep_us); + } + break; + default: + printf("EXITING...\n"); + return -1; + } + + if (0 > transport_node->init()) + { + printf("EXITING...\n"); + return -1; + } + + sleep(1); + +@[if send_topics]@ + char data_buffer[BUFFER_SIZE] = {}; + int received = 0, loop = 0; + int length = 0, total_read = 0; + bool receiving = false; + uint8_t topic_ID = 255; + std::chrono::time_point start, end; +@[end if]@ + + topics.init(); + + running = true; +@[if recv_topics]@ + std::thread sender_thread(t_send, nullptr); +@[end if]@ + + while (running) + { +@[if send_topics]@ + ++loop; + if (!receiving) start = std::chrono::steady_clock::now(); + // Publish messages received from UART + while (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) + { + topics.publish(topic_ID, data_buffer, sizeof(data_buffer)); + ++received; + total_read += length; + receiving = true; + end = std::chrono::steady_clock::now(); + } + + if ((receiving && std::chrono::duration(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) || + (!running && loop > 1)) + { + std::chrono::duration elapsed_secs = end - start; + printf("\nSENT: %lu messages - %lu bytes\n", + (unsigned long)sent, (unsigned long)total_sent); + printf("RECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n\n", + received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count())); + received = sent = total_read = total_sent = 0; + receiving = false; + } + +@[end if]@ + usleep(_options.sleep_us); + } +@[if recv_topics]@ + sender_thread.join(); +@[end if]@ + delete transport_node; + transport_node = nullptr; + + return 0; +} diff --git a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template new file mode 100644 index 000000000000..200baecc8ba7 --- /dev/null +++ b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template @@ -0,0 +1,54 @@ +################################################################################ +# +# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +################################################################################ + +cmake_minimum_required(VERSION 2.8.12) +project(micrortps_agent) + +# Find requirements +find_package(fastrtps REQUIRED) +find_package(fastcdr REQUIRED) + +# Set C++11 +include(CheckCXXCompilerFlag) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR + CMAKE_CXX_COMPILER_ID MATCHES "Clang") + check_cxx_compiler_flag(--std=c++11 SUPPORTS_CXX11) + if(SUPPORTS_CXX11) + add_compile_options(--std=c++11) + else() + message(FATAL_ERROR "Compiler doesn't support C++11") + endif() +endif() + +file(GLOB MICRORTPS_AGENT_SOURCES *.cpp) +add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES}) +target_link_libraries(micrortps_agent fastrtps fastcdr) diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp new file mode 100644 index 000000000000..de97d7d61c6b --- /dev/null +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -0,0 +1,503 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include + +#include "microRTPS_transport.h" + +#define DEFAULT_UART "/dev/ttyACM0" + +/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ +uint16_t const crc16_table[256] = { + 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, + 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, + 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, + 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, + 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, + 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, + 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, + 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, + 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, + 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, + 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, + 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, + 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, + 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, + 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, + 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, + 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, + 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, + 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, + 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, + 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, + 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, + 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, + 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, + 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, + 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, + 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, + 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, + 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, + 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, + 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, + 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 +}; + +Transport_node::Transport_node(): rx_buff_pos(0) +{ +} + +Transport_node::~Transport_node() +{ +} + +uint16_t Transport_node::crc16_byte(uint16_t crc, const uint8_t data) +{ + return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff]; +} + +uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len) +{ + uint16_t crc = 0; + + while (len--) { + crc = crc16_byte(crc, *buffer++); + } + + return crc; +} + +ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer_len) +{ + if (nullptr == out_buffer || nullptr == topic_ID || !fds_OK()) { + return -1; + } + + *topic_ID = 255; + + ssize_t len = node_read((void *)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos); + + if (len <= 0) { + int errsv = errno; + + if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { + printf("Read fail %d\n", errsv); + } + + return len; + } + + rx_buff_pos += len; + + // We read some + size_t header_size = sizeof(struct Header); + + // but not enough + if (rx_buff_pos < header_size) { + return 0; + } + + uint32_t msg_start_pos = 0; + + for (msg_start_pos = 0; msg_start_pos <= rx_buff_pos - header_size; ++msg_start_pos) { + if ('>' == rx_buffer[msg_start_pos] && memcmp(rx_buffer + msg_start_pos, ">>>", 3) == 0) { + break; + } + } + + // Start not found + if (msg_start_pos > rx_buff_pos - header_size) { + printf(" (↓↓ %u)\n", msg_start_pos); + // All we've checked so far is garbage, drop it - but save unchecked bytes + memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); + rx_buff_pos = rx_buff_pos - msg_start_pos; + return -1; + } + + /* + * [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] + */ + + struct Header *header = (struct Header *)&rx_buffer[msg_start_pos]; + uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; + + // The message won't fit the buffer. + if (buffer_len < header_size + payload_len) { + return -EMSGSIZE; + } + + // We do not have a complete message yet + if (msg_start_pos + header_size + payload_len > rx_buff_pos) { + // If there's garbage at the beginning, drop it + if (msg_start_pos > 0) { + printf(" (↓ %u)\n", msg_start_pos); + memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); + rx_buff_pos -= msg_start_pos; + } + + return 0; + } + + uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; + uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len); + + if (read_crc != calc_crc) { + printf("BAD CRC %u != %u\n", read_crc, calc_crc); + printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len)); + len = -1; + + } else { + // copy message to outbuffer and set other return values + memmove(out_buffer, rx_buffer + msg_start_pos + header_size, payload_len); + *topic_ID = header->topic_ID; + len = payload_len + header_size; + } + + // discard message from rx_buffer + rx_buff_pos -= header_size + payload_len; + memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos); + + return len; +} + +ssize_t Transport_node::get_header_length() +{ + return sizeof(struct Header); +} + +ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t length) +{ + if (!fds_OK()) { + return -1; + } + + static struct Header header = { + .marker = {'>', '>', '>'}, + .topic_ID = 0u, + .seq = 0u, + .payload_len_h = 0u, + .payload_len_l = 0u, + .crc_h = 0u, + .crc_l = 0u + + }; + + static uint8_t seq = 0; + + // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] + + uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); + + header.topic_ID = topic_ID; + header.seq = seq++; + header.payload_len_h = (length >> 8) & 0xff; + header.payload_len_l = length & 0xff; + header.crc_h = (crc >> 8) & 0xff; + header.crc_l = crc & 0xff; + + /* Headroom for header is created in client */ + /*Fill in the header in the same payload buffer to call a single node_write */ + memcpy(buffer, &header, sizeof(header)); + ssize_t len = node_write(buffer, length + sizeof(header)); + if (len != ssize_t(length + sizeof(header))) { + goto err; + } + return len + sizeof(header); + +err: + //int errsv = errno; + //if (len == -1 ) printf(" => Writing error '%d'\n", errsv); + //else printf(" => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv); + + return len; +} + +UART_node::UART_node(const char *_uart_name, uint32_t _baudrate, uint32_t _poll_ms): + uart_fd(-1), + baudrate(_baudrate), + poll_ms(_poll_ms) + +{ + if (nullptr != _uart_name) { + strcpy(uart_name, _uart_name); + } +} + +UART_node::~UART_node() +{ + close(); +} + +int UART_node::init() +{ + // Open a serial port + uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (uart_fd < 0) { + printf("failed to open device: %s (%d)\n", uart_name, errno); + return -errno; + } + + // If using shared UART, no need to set it up + if (baudrate == 0) { + return uart_fd; + } + + // Try to set baud rate + struct termios uart_config; + int termios_state; + + // Back up the original uart configuration to restore it after exit + if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { + int errno_bkp = errno; + printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno); + close(); + return -errno_bkp; + } + + // Clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; + + // USB serial is indicated by /dev/ttyACM0 + if (strcmp(uart_name, "/dev/ttyACM0") != 0 && strcmp(uart_name, "/dev/ttyACM1") != 0) { + // Set baud rate + if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0) { + int errno_bkp = errno; + printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno); + close(); + return -errno_bkp; + } + } + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + int errno_bkp = errno; + printf("ERR SET CONF %s (%d)\n", uart_name, errno); + close(); + return -errno_bkp; + } + + char aux[64]; + bool flush = false; + + while (0 < ::read(uart_fd, (void *)&aux, 64)) { + //printf("%s ", aux); + flush = true; + usleep(1000); + } + + if (flush) { + printf("flush\n"); + + } else { + printf("no flush\n"); + } + + poll_fd[0].fd = uart_fd; + poll_fd[0].events = POLLIN; + + return uart_fd; +} + +bool UART_node::fds_OK() +{ + return (-1 != uart_fd); +} + +uint8_t UART_node::close() +{ + if (-1 != uart_fd) { + printf("Close UART\n"); + ::close(uart_fd); + uart_fd = -1; + memset(&poll_fd, 0, sizeof(poll_fd)); + } + + return 0; +} + +ssize_t UART_node::node_read(void *buffer, size_t len) +{ + if (nullptr == buffer || !fds_OK()) { + return -1; + } + + ssize_t ret = 0; + int r = poll(poll_fd, 1, poll_ms); + + if (r == 1 && (poll_fd[0].revents & POLLIN)) { + ret = ::read(uart_fd, buffer, len); + } + + return ret; +} + +ssize_t UART_node::node_write(void *buffer, size_t len) +{ + if (nullptr == buffer || !fds_OK()) { + return -1; + } + + return ::write(uart_fd, buffer, len); +} + + +UDP_node::UDP_node(uint16_t _udp_port_recv, uint16_t _udp_port_send): + sender_fd(-1), + receiver_fd(-1), + udp_port_recv(_udp_port_recv), + udp_port_send(_udp_port_send) +{ +} + +UDP_node::~UDP_node() +{ + close(); +} + +int UDP_node::init() +{ + if (0 > init_receiver(udp_port_recv) || 0 > init_sender(udp_port_send)) { + return -1; + } + + return 0; +} + +bool UDP_node::fds_OK() +{ + return (-1 != sender_fd && -1 != receiver_fd); +} + +int UDP_node::init_receiver(uint16_t udp_port) +{ +#ifndef __PX4_NUTTX + // udp socket data + memset((char *)&receiver_inaddr, 0, sizeof(receiver_inaddr)); + receiver_inaddr.sin_family = AF_INET; + receiver_inaddr.sin_port = htons(udp_port); + receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + printf("create socket failed\n"); + return -1; + } + + printf("Trying to connect...\n"); + + if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { + printf("bind failed\n"); + return -1; + } + + printf("connected to server!\n"); +#endif /* __PX4_NUTTX */ + + return 0; +} + +int UDP_node::init_sender(uint16_t udp_port) +{ +#ifndef __PX4_NUTTX + + if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + printf("create socket failed\n"); + return -1; + } + + memset((char *) &sender_outaddr, 0, sizeof(sender_outaddr)); + sender_outaddr.sin_family = AF_INET; + sender_outaddr.sin_port = htons(udp_port); + + if (inet_aton("127.0.0.1", &sender_outaddr.sin_addr) == 0) { + printf("inet_aton() failed\n"); + return -1; + } + +#endif /* __PX4_NUTTX */ + + return 0; +} + +uint8_t UDP_node::close() +{ +#ifndef __PX4_NUTTX + + if (sender_fd != -1) { + printf("Close sender socket\n"); + shutdown(sender_fd, SHUT_RDWR); + ::close(sender_fd); + sender_fd = -1; + } + + if (receiver_fd != -1) { + printf("Close receiver socket\n"); + shutdown(receiver_fd, SHUT_RDWR); + ::close(receiver_fd); + receiver_fd = -1; + } + +#endif /* __PX4_NUTTX */ + return 0; +} + +ssize_t UDP_node::node_read(void *buffer, size_t len) +{ + if (nullptr == buffer || !fds_OK()) { + return -1; + } + + int ret = 0; +#ifndef __PX4_NUTTX + // Blocking call + static socklen_t addrlen = sizeof(receiver_outaddr); + ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr *) &receiver_outaddr, &addrlen); +#endif /* __PX4_NUTTX */ + return ret; +} + +ssize_t UDP_node::node_write(void *buffer, size_t len) +{ + if (nullptr == buffer || !fds_OK()) { + return -1; + } + + int ret = 0; +#ifndef __PX4_NUTTX + ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr)); +#endif /* __PX4_NUTTX */ + return ret; +} diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h new file mode 100644 index 000000000000..03d47670f9f9 --- /dev/null +++ b/msg/templates/urtps/microRTPS_transport.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +class Transport_node +{ +public: + Transport_node(); + virtual ~Transport_node(); + + virtual int init() {return 0;} + virtual uint8_t close() {return 0;} + ssize_t read(uint8_t *topic_ID, char out_buffer[], size_t buffer_len); + + /** + * write a buffer + * @param topic_ID + * @param buffer buffer to write: it must leave get_header_length() bytes free at the beginning. This will be + * filled with the header. length does not include get_header_length(). So buffer looks like this: + * ------------------------------------------------- + * | header (leave free) | payload data | + * | get_header_length() bytes | length bytes | + * ------------------------------------------------- + * @param length buffer length excluding header length + * @return length on success, <0 on error + */ + ssize_t write(const uint8_t topic_ID, char buffer[], size_t length); + + /** Get the Length of struct Header to make headroom for the size of struct Header along with payload */ + ssize_t get_header_length(); + +protected: + virtual ssize_t node_read(void *buffer, size_t len) = 0; + virtual ssize_t node_write(void *buffer, size_t len) = 0; + virtual bool fds_OK() = 0; + uint16_t crc16_byte(uint16_t crc, const uint8_t data); + uint16_t crc16(uint8_t const *buffer, size_t len); + +protected: + uint32_t rx_buff_pos; + char rx_buffer[1024] = {}; + +private: + struct __attribute__((packed)) Header { + char marker[3]; + uint8_t topic_ID; + uint8_t seq; + uint8_t payload_len_h; + uint8_t payload_len_l; + uint8_t crc_h; + uint8_t crc_l; + }; +}; + +class UART_node: public Transport_node +{ +public: + UART_node(const char *uart_name, uint32_t baudrate, uint32_t poll_ms); + virtual ~UART_node(); + + int init(); + uint8_t close(); + +protected: + ssize_t node_read(void *buffer, size_t len); + ssize_t node_write(void *buffer, size_t len); + bool fds_OK(); + + int uart_fd; + char uart_name[64] = {}; + uint32_t baudrate; + uint32_t poll_ms; + struct pollfd poll_fd[1] = {}; +}; + +class UDP_node: public Transport_node +{ +public: + UDP_node(uint16_t udp_port_recv, uint16_t udp_port_send); + virtual ~UDP_node(); + + int init(); + uint8_t close(); + +protected: + int init_receiver(uint16_t udp_port); + int init_sender(uint16_t udp_port); + ssize_t node_read(void *buffer, size_t len); + ssize_t node_write(void *buffer, size_t len); + bool fds_OK(); + + int sender_fd; + int receiver_fd; + uint16_t udp_port_recv; + uint16_t udp_port_send; + struct sockaddr_in sender_outaddr; + struct sockaddr_in receiver_inaddr; + struct sockaddr_in receiver_outaddr; +}; diff --git a/msg/templates/urtps/msg.idl.template b/msg/templates/urtps/msg.idl.template new file mode 100644 index 000000000000..8e30eaebc77b --- /dev/null +++ b/msg/templates/urtps/msg.idl.template @@ -0,0 +1,75 @@ +@############################################### +@# +@# ROS message to IDL converter +@# +@# EmPy template for generating .idl files +@# +@################################################################################ +@# +@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +@# +@# Redistribution and use in source and binary forms, with or without +@# modification, are permitted provided that the following conditions are met: +@# +@# 1. Redistributions of source code must retain the above copyright notice, this +@# list of conditions and the following disclaimer. +@# +@# 2. Redistributions in binary form must reproduce the above copyright notice, +@# this list of conditions and the following disclaimer in the documentation +@# and/or other materials provided with the distribution. +@# +@# 3. Neither the name of the copyright holder nor the names of its contributors +@# may be used to endorse or promote products derived from this software without +@# specific prior written permission. +@# +@# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +@# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +@# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +@# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +@# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +@# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +@# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +@# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +@# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +@# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +@# POSSIBILITY OF SUCH DAMAGE. +@# +@################################################################################ +@{ +import genmsg.msgs +import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ +}@ +@################################################# +@# Searching for serialize function per each field +@################################################# +@{ + +def get_idl_type_name(field_type): + if field_type in type_idl_map: + return type_idl_map[field_type] + else: + (package, name) = genmsg.names.package_resource_name(field_type) + return name + +def add_msg_field(field): + if (not field.is_header): + if (not field.is_array): + print(" " + str(get_idl_type_name(field.type)) + ' ' + field.name + ';') + else: + print(" " + str(get_idl_type_name(field.base_type)) + ' ' + field.name + '[' +str(field.array_len)+ '];') + +def add_msg_fields(): + # sort fields (using a stable sort) as in the declaration of the type + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + for field in sorted_fields: + add_msg_field(field) + + +}@ +@ +struct @(spec.short_name)_ +{ + unsigned long long timestamp; +@add_msg_fields() +}; // struct @(spec.short_name)_ diff --git a/msg/test_motor.msg b/msg/test_motor.msg new file mode 100644 index 000000000000..2805fa64f510 --- /dev/null +++ b/msg/test_motor.msg @@ -0,0 +1,4 @@ +uint8 NUM_MOTOR_OUTPUTS = 8 + +uint32 motor_number # number of motor to spin +float32 value # output power, range [0..1] diff --git a/msg/time_offset.msg b/msg/time_offset.msg new file mode 100644 index 000000000000..da2d89ab2402 --- /dev/null +++ b/msg/time_offset.msg @@ -0,0 +1 @@ +uint64 offset_ns # time offset between companion system and PX4, in nanoseconds diff --git a/msg/tools/gencpp b/msg/tools/gencpp new file mode 160000 index 000000000000..26a86f04bcec --- /dev/null +++ b/msg/tools/gencpp @@ -0,0 +1 @@ +Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py new file mode 100644 index 000000000000..86d212ac9f88 --- /dev/null +++ b/msg/tools/generate_microRTPS_bridge.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python + +################################################################################ +# +# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +################################################################################ + +# This script can generate the client and agent code based on a set of topics +# to sent and set to receive. It uses fastrtpsgen to generate the code from the +# IDL for the topic messages. The PX4 msg definitions are used to create the IDL +# used by fastrtpsgen using templates. + +import sys, os, argparse, shutil +import px_generate_uorb_topic_files +import subprocess, glob +import errno + +def get_absolute_path(arg_parse_dir): + root_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + + if isinstance(arg_parse_dir, list): + dir = arg_parse_dir[0] + else: + dir = arg_parse_dir + + if dir[0] != '/': + dir = root_path + "/" + dir + + return dir + +default_client_out = get_absolute_path("src/modules/micrortps_bridge/micrortps_client") +default_agent_out = get_absolute_path("src/modules/micrortps_bridge/micrortps_agent") + +parser = argparse.ArgumentParser() +parser.add_argument("-s", "--send", dest='send', metavar='*.msg', type=str, nargs='+', help="Topics to be sended") +parser.add_argument("-r", "--receive", dest='receive', metavar='*.msg', type=str, nargs='+', help="Topics to be received") +parser.add_argument("-a", "--agent", dest='agent', action="store_true", help="Flag for generate the agent, by default is true if -c is not specified") +parser.add_argument("-c", "--client", dest='client', action="store_true", help="Flag for generate the client, by default is true if -a is not specified") +parser.add_argument("-t", "--topic-msg-dir", dest='msgdir', type=str, nargs=1, help="Topics message dir, by default msg/", default="msg") +parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, nargs=1, help="Agent output dir, by default src/modules/micrortps_bridge/micrortps_agent", default=default_agent_out) +parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, nargs=1, help="Client output dir, by default src/modules/micrortps_bridge/micrortps_client", default=default_client_out) +parser.add_argument("-f", "--fastrtpsgen-dir", dest='fastrtpsgen', type=str, nargs='?', help="fastrtpsgen installation dir, only needed if fastrtpsgen is not in PATH, by default empty", default="") +parser.add_argument("--delete-tree", dest='del_tree', action="store_true", help="Delete dir tree output dir(s)") + +if len(sys.argv) <= 1: + parser.print_usage() + exit(-1) + +# Parse arguments +args = parser.parse_args() +msg_folder = get_absolute_path(args.msgdir) +msg_files_send = [] +if args.send: + msg_files_send = [get_absolute_path(msg) for msg in args.send] +else: + msg_files_send = [] +if args.receive: + msg_files_receive = [get_absolute_path(msg) for msg in args.receive] +else: + msg_files_receive = [] +agent = args.agent +client = args.client +del_tree = args.del_tree +px_generate_uorb_topic_files.append_to_include_path({msg_folder}, px_generate_uorb_topic_files.INCL_DEFAULT) +agent_out_dir = get_absolute_path(args.agentdir) +client_out_dir = get_absolute_path(args.clientdir) + +if args.fastrtpsgen is None or args.fastrtpsgen == "": + # Assume fastrtpsgen is in PATH + fastrtpsgen_path = "fastrtpsgen" +else: + # Path to fastrtpsgen is explicitly specified + fastrtpsgen_path = get_absolute_path(args.fastrtpsgen) + "/fastrtpsgen" + +# If nothing specified it's generated both +if agent == False and client == False: + agent = True + client = True + +if del_tree: + if agent: + _continue = str(raw_input("\nFiles in " + agent_out_dir + " will be erased, continue?[Y/n]\n")) + if _continue == "N" or _continue == "n": + print("Aborting execution...") + exit(-1) + else: + if agent and os.path.isdir(agent_out_dir): + shutil.rmtree(agent_out_dir) + + if client: + _continue = str(raw_input("\nFiles in " + client_out_dir + " will be erased, continue?[Y/n]\n")) + if _continue == "N" or _continue == "n": + print("Aborting execution...") + exit(-1) + else: + if client and os.path.isdir(client_out_dir): + shutil.rmtree(client_out_dir) + +if agent and os.path.isdir(agent_out_dir + "/idl"): + shutil.rmtree(agent_out_dir + "/idl") + +uorb_templates_dir = msg_folder + "/templates/uorb_microcdr" +urtps_templates_dir = msg_folder + "/templates/urtps" + +uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.template' +uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.template' +uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cpp.template' +uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cpp.template' +uRTPS_AGENT_CMAKELIST_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.template' +uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cpp.template' +uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.template' +uRTPS_SUBSCRIBER_SRC_TEMPL_FILE = 'Subscriber.cpp.template' +uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.template' + +def generate_agent(out_dir): + + if msg_files_send: + for msg_file in msg_files_send: + px_generate_uorb_topic_files.generate_idl_file(msg_file, out_dir + "/idl", urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT) + px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_PUBLISHER_SRC_TEMPL_FILE) + px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_PUBLISHER_H_TEMPL_FILE) + + if msg_files_receive: + for msg_file in msg_files_receive: + px_generate_uorb_topic_files.generate_idl_file(msg_file, out_dir + "/idl", urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT) + px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) + px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_SUBSCRIBER_H_TEMPL_FILE) + + px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TEMPL_FILE) + px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) + px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) + px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_CMAKELIST_TEMPL_FILE) + + # Final steps to install agent + mkdir_p(agent_out_dir + "/fastrtpsgen") + os.chdir(agent_out_dir + "/fastrtpsgen") + for idl_file in glob.glob(agent_out_dir + "/idl/*.idl"): + ret = subprocess.call(fastrtpsgen_path + " -d " + agent_out_dir + "/fastrtpsgen -example x64Linux2.6gcc " + idl_file, shell=True) + if ret: + raise Exception("fastrtpsgen not found. Specify the location of fastrtpsgen with the -f flag") + rm_wildcard(agent_out_dir + "/fastrtpsgen/*PubSubMain*") + rm_wildcard(agent_out_dir + "/fastrtpsgen/makefile*") + rm_wildcard(agent_out_dir + "/fastrtpsgen/*Publisher*") + rm_wildcard(agent_out_dir + "/fastrtpsgen/*Subscriber*") + for f in glob.glob(agent_out_dir + "/fastrtpsgen/*.cxx"): + os.rename(f, f.replace(".cxx", ".cpp")) + cp_wildcard(agent_out_dir + "/fastrtpsgen/*", agent_out_dir) + if os.path.isdir(agent_out_dir + "/fastrtpsgen"): + shutil.rmtree(agent_out_dir + "/fastrtpsgen") + cp_wildcard(urtps_templates_dir + "/microRTPS_transport.*", agent_out_dir) + os.rename(agent_out_dir + "/microRTPS_agent_CMakeLists.txt", agent_out_dir + "/CMakeLists.txt") + mkdir_p(agent_out_dir + "/build") + return 0 + +def rm_wildcard(pattern): + for f in glob.glob(pattern): + os.remove(f) + +def cp_wildcard(pattern, destdir): + for f in glob.glob(pattern): + shutil.copy(f, destdir) + +def mkdir_p(dirpath): + try: + os.makedirs(dirpath) + except OSError as e: + if e.errno == errno.EEXIST and os.path.isdir(dirpath): + pass + else: + raise + +def generate_client(out_dir): + + # Rename work in the default path + if default_client_out != out_dir: + def_file = default_client_out + "/microRTPS_client.cpp" + if os.path.isfile(def_file): + os.rename(def_file, def_file.replace(".cpp", ".cpp_")) + def_file = default_client_out + "/microRTPS_transport.cpp" + if os.path.isfile(def_file): + os.rename(def_file, def_file.replace(".cpp", ".cpp_")) + def_file = default_client_out + "/microRTPS_transport.h" + if os.path.isfile(def_file): + os.rename(def_file, def_file.replace(".h", ".h_")) + + px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, uorb_templates_dir, + px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_CLIENT_TEMPL_FILE) + + # Final steps to install client + cp_wildcard(urtps_templates_dir + "/microRTPS_transport.*", out_dir) + + return 0 + +if agent: + generate_agent(agent_out_dir) + print("\nAgent created in: " + agent_out_dir) + +if client: + generate_client(client_out_dir) + print("\nClient created in: " + client_out_dir) diff --git a/msg/tools/genmsg b/msg/tools/genmsg new file mode 160000 index 000000000000..72f0383f0e6a --- /dev/null +++ b/msg/tools/genmsg @@ -0,0 +1 @@ +Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454 diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py new file mode 100755 index 000000000000..a519431f60c6 --- /dev/null +++ b/msg/tools/px_generate_uorb_topic_files.py @@ -0,0 +1,440 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_uorb_topics.py +Generates c/cpp header/source files for uorb topics from .msg (ROS syntax) +message files +""" +from __future__ import print_function +import os +import shutil +import filecmp +import argparse +import sys + +px4_tools_dir = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(px4_tools_dir + "/genmsg/src") +sys.path.append(px4_tools_dir + "/gencpp/src") +px4_msg_dir = os.path.join(px4_tools_dir, "..") + +try: + import em + import genmsg.template_tools +except ImportError as e: + print("python import error: ", e) + print(''' +Required python packages not installed. + +On a Debian/Ubuntu system please run: + + sudo apt-get install python-empy + sudo pip install catkin_pkg + +On MacOS please run: + sudo pip install empy catkin_pkg + +On Windows please run: + easy_install empy catkin_pkg +''') + exit(1) + +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2016 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + + +TEMPLATE_FILE = ['msg.h.template', 'msg.cpp.template'] +TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template' +OUTPUT_FILE_EXT = ['.h', '.cpp'] +INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] +PACKAGE = 'px4' +TOPICS_TOKEN = '# TOPICS ' +IDL_TEMPLATE_FILE = 'msg.idl.template' + +class MsgScope: + NONE = 0 + SEND = 1 + RECEIVE = 2 + +def get_multi_topics(filename): + """ + Get TOPICS names from a "# TOPICS" line + """ + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith (TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + result.extend(topic_names_str.split(" ")) + ofile.close() + return result + +def get_msgs_list(msgdir): + """ + Makes list of msg files in the given directory + """ + return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] + + +def generate_output_from_file(format_idx, filename, outputdir, templatedir, includepath): + """ + Converts a single .msg file to an uorb header/source file + """ + msg_context = genmsg.msg_loader.MsgContext.create_default() + full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename)) + spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name) + topics = get_multi_topics(filename) + if includepath: + search_path = genmsg.command_line.includepath_to_dict(includepath) + else: + search_path = {} + genmsg.msg_loader.load_depends(msg_context, spec, search_path) + md5sum = genmsg.gentools.compute_md5(msg_context, spec) + if len(topics) == 0: + topics.append(spec.short_name) + em_globals = { + "file_name_in": filename, + "md5sum": md5sum, + "search_path": search_path, + "msg_context": msg_context, + "spec": spec, + "topics": topics + } + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) + output_file = os.path.join(outputdir, spec.short_name + + OUTPUT_FILE_EXT[format_idx]) + + return generate_by_template(output_file, template_file, em_globals) + +def generate_idl_file(filename_msg, outputdir, templatedir, includepath): + """ + Generates an .idl from .msg file + """ + em_globals = get_em_globals(filename_msg, includepath, MsgScope.NONE) + spec_short_name = em_globals["spec"].short_name + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + template_file = os.path.join(templatedir, IDL_TEMPLATE_FILE) + output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace("msg.idl.template", str(spec_short_name + "_.idl"))) + + return generate_by_template(output_file, template_file, em_globals) + +def generate_uRTPS_general(filename_send_msgs, filename_received_msgs, + outputdir, templatedir, includepath, template_name): + """ + Generates source file by msg content + """ + em_globals_list = [] + if filename_send_msgs: + em_globals_list.extend([get_em_globals(f, includepath, MsgScope.SEND) for f in filename_send_msgs]) + + if filename_received_msgs: + em_globals_list.extend([get_em_globals(f, includepath, MsgScope.RECEIVE) for f in filename_received_msgs]) + + merged_em_globals = merge_em_globals_list(em_globals_list) + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + template_file = os.path.join(templatedir, template_name) + output_file = os.path.join(outputdir, template_name.replace(".template", "")) + + return generate_by_template(output_file, template_file, merged_em_globals) + +def generate_topic_file(filename_msg, outputdir, templatedir, includepath, template_name): + """ + Generates an .idl from .msg file + """ + em_globals = get_em_globals(filename_msg, includepath, MsgScope.NONE) + spec_short_name = em_globals["spec"].short_name + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + template_file = os.path.join(templatedir, template_name) + output_file = os.path.join(outputdir, spec_short_name + "_" + template_name.replace(".template", "")) + + return generate_by_template(output_file, template_file, em_globals) + +def get_em_globals(filename_msg, includepath, scope): + """ + Generates em globals dictionary + """ + msg_context = genmsg.msg_loader.MsgContext.create_default() + full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg)) + spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name) + topics = get_multi_topics(filename_msg) + if includepath: + search_path = genmsg.command_line.includepath_to_dict(includepath) + else: + search_path = {} + genmsg.msg_loader.load_depends(msg_context, spec, search_path) + md5sum = genmsg.gentools.compute_md5(msg_context, spec) + if len(topics) == 0: + topics.append(spec.short_name) + em_globals = { + "file_name_in": filename_msg, + "md5sum": md5sum, + "search_path": search_path, + "msg_context": msg_context, + "spec": spec, + "topics": topics, + "scope": scope + } + + return em_globals + +def merge_em_globals_list(em_globals_list): + """ + Merges a list of em_globals to a single dictionary where each attribute is a list + """ + if len(em_globals_list) < 1: + return {} + + merged_em_globals = {} + for name in em_globals_list[0]: + merged_em_globals[name] = [em_globals[name] for em_globals in em_globals_list] + + return merged_em_globals + + + +def generate_by_template(output_file, template_file, em_globals): + """ + Invokes empy intepreter to geneate output_file by the + given template_file and predefined em_globals dict + """ + # check if folder exists: + folder_name = os.path.dirname(output_file) + if not os.path.exists(folder_name): + os.makedirs(folder_name) + + ofile = open(output_file, 'w') + # todo, reuse interpreter + interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True}) + if not os.path.isfile(template_file): + ofile.close() + os.remove(output_file) + raise RuntimeError("Template file %s not found" % (template_file)) + interpreter.file(open(template_file)) #todo try + interpreter.shutdown() + ofile.close() + return True + + +def convert_dir(format_idx, inputdir, outputdir, templatedir): + """ + Converts all .msg files in inputdir to uORB header/source files + """ + + # Find the most recent modification time in input dir + maxinputtime = 0 + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxinputtime: + maxinputtime = it + + # Find the most recent modification time in output dir + maxouttime = 0 + if os.path.isdir(outputdir): + for f in os.listdir(outputdir): + fni = os.path.join(outputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxouttime: + maxouttime = it + + # Do not generate if nothing changed on the input + if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): + return False + + includepath = INCL_DEFAULT + [':'.join([PACKAGE, inputdir])] + for f in os.listdir(inputdir): + # Ignore hidden files + if f.startswith("."): + continue + + fn = os.path.join(inputdir, f) + # Only look at actual files + if not os.path.isfile(fn): + continue + + if fn[-4:].lower() != '.msg': + continue + + generate_output_from_file(format_idx, fn, outputdir, templatedir, includepath) + return True + + +def copy_changed(inputdir, outputdir, prefix='', quiet=False): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + for input_file in os.listdir(inputdir): + fni = os.path.join(inputdir, input_file) + if os.path.isfile(fni): + # Check if input_file exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, prefix + input_file) + if not os.path.isfile(fno): + shutil.copy(fni, fno) + if not quiet: + print("{0}: new header file".format(fno)) + continue + + if os.path.getmtime(fni) > os.path.getmtime(fno): + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + if not quiet: + print("{0}: updated".format(input_file)) + continue + + if not quiet: + print("{0}: unchanged".format(input_file)) + + +def convert_dir_save(format_idx, inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False): + """ + Converts all .msg files in inputdir to uORB header files + Unchanged existing files are not overwritten. + """ + # Create new headers in temporary output directory + convert_dir(format_idx, inputdir, temporarydir, templatedir) + if generate_idx == 1: + generate_topics_list_file(inputdir, temporarydir, templatedir) + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir, prefix, quiet) + +def generate_topics_list_file(msgdir, outputdir, templatedir): + # generate cpp file with topics list + msgs = get_msgs_list(msgdir) + multi_topics = [] + for msg in msgs: + msg_filename = os.path.join(msgdir, msg) + multi_topics.extend(get_multi_topics(msg_filename)) + tl_globals = {"msgs" : msgs, "multi_topics" : multi_topics} + tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE) + tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", "")) + generate_by_template(tl_out_file, tl_template_file, tl_globals) + +def generate_topics_list_file_from_files(files, outputdir, templatedir): + # generate cpp file with topics list + filenames = [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")] + multi_topics = [] + for msg_filename in files: + multi_topics.extend(get_multi_topics(msg_filename)) + tl_globals = {"msgs" : filenames, "multi_topics" : multi_topics} + tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE) + tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", "")) + generate_by_template(tl_out_file, tl_template_file, tl_globals) + +def append_to_include_path(path_to_append, curr_include): + for p in path_to_append: + curr_include.append("%s:%s" % (PACKAGE, p)) + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description='Convert msg files to uorb headers/sources') + parser.add_argument('--headers', help='Generate header files', + action='store_true') + parser.add_argument('--sources', help='Generate source files', + action='store_true') + parser.add_argument('-d', dest='dir', help='directory with msg files') + parser.add_argument('-f', dest='file', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-i', dest="include_paths", + help='Additional Include Paths', nargs="*", + default=None) + parser.add_argument('-e', dest='templatedir', + help='directory with template files',) + parser.add_argument('-o', dest='outputdir', + help='output directory for header files') + parser.add_argument('-t', dest='temporarydir', + help='temporary directory') + parser.add_argument('-p', dest='prefix', default='', + help='string added as prefix to the output file ' + ' name when converting directories') + parser.add_argument('-q', dest='quiet', default=False, action='store_true', + help='string added as prefix to the output file ' + ' name when converting directories') + args = parser.parse_args() + + if args.include_paths: + append_to_include_path(args.include_paths, INCL_DEFAULT) + + if args.headers: + generate_idx = 0 + elif args.sources: + generate_idx = 1 + else: + print('Error: either --headers or --sources must be specified') + exit(-1) + if args.file is not None: + for f in args.file: + generate_output_from_file(generate_idx, f, args.temporarydir, args.templatedir, INCL_DEFAULT) + if generate_idx == 1: + generate_topics_list_file_from_files(args.file, args.outputdir, args.templatedir) + copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet) + elif args.dir is not None: + convert_dir_save( + generate_idx, + args.dir, + args.outputdir, + args.templatedir, + args.temporarydir, + args.prefix, + args.quiet) diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py new file mode 100644 index 000000000000..7f55b0ee6af0 --- /dev/null +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -0,0 +1,201 @@ + +# helper methods & common code for the uorb message templates msg.{cpp,h}.template + +# Another positive effect of having the code here, is that this file will get +# precompiled and thus message generation will be much faster + + +import genmsg.msgs +import gencpp + +type_map = { + 'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'float64': 'double', + 'bool': 'bool', + 'char': 'char', +} + +type_serialize_map = { + 'int8': 'SignedChar', + 'int16': 'Short', + 'int32': 'Int', + 'int64': 'Long', + 'uint8': 'UnsignedChar', + 'uint16': 'UnsignedShort', + 'uint32': 'UnsignedInt', + 'uint64': 'UnsignedLong', + 'float32': 'Float', + 'float64': 'Double', + 'bool': 'Bool', + 'char': 'Char', +} + +type_idl_map = { + 'int8': 'octet', + 'int16': 'short', + 'int32': 'long', + 'int64': 'long long', + 'uint8': 'octet', + 'uint16': 'unsigned short', + 'uint32': 'unsigned long', + 'uint64': 'unsigned long long', + 'float32': 'float', + 'float64': 'double', + 'bool': 'boolean', + 'char': 'char', +} + +msgtype_size_map = { + 'int8': 1, + 'int16': 2, + 'int32': 4, + 'int64': 8, + 'uint8': 1, + 'uint16': 2, + 'uint32': 4, + 'uint64': 8, + 'float32': 4, + 'float64': 8, + 'bool': 1, + 'char': 1, +} + + +def bare_name(msg_type): + """ + Get bare_name from /[x] format + """ + bare = msg_type + if '/' in msg_type: + # removing prefix + bare = (msg_type.split('/'))[1] + # removing suffix + return bare.split('[')[0] + + +def sizeof_field_type(field): + """ + Get size of a field, used for sorting + """ + bare_name_str = bare_name(field.type) + if bare_name_str in msgtype_size_map: + return msgtype_size_map[bare_name_str] + return 0 # this is for non-builtin types: sort them at the end + +def get_children_fields(base_type, search_path): + (package, name) = genmsg.names.package_resource_name(base_type) + tmp_msg_context = genmsg.msg_loader.MsgContext.create_default() + spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path) + sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True) + return sorted_fields + +def add_padding_bytes(fields, search_path): + """ + Add padding fields before the embedded types, at the end and calculate the + struct size + returns a tuple with the struct size and padding at the end + """ + struct_size = 8 # account for the timestamp + align_to = 8 # this is always 8, because of the 64bit timestamp + i = 0 + padding_idx = 0 + while i < len(fields): + field = fields[i] + if not field.is_header: + a_pos = field.type.find('[') + array_size = 1 + if field.is_array: + array_size = field.array_len + if field.is_builtin: + field.sizeof_field_type = sizeof_field_type(field) + else: + # embedded type: may need to add padding + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes != align_to: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.insert(i, padding_field) + i += 1 + children_fields = get_children_fields(field.base_type, search_path) + field.sizeof_field_type, unused = add_padding_bytes(children_fields, + search_path) + struct_size += field.sizeof_field_type * array_size + i += 1 + + # add padding at the end (necessary for embedded types) + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes == align_to: + num_padding_bytes = 0 + else: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.append(padding_field) + return (struct_size, num_padding_bytes) + + +def convert_type(spec_type): + """ + Convert from msg type to C type + """ + bare_type = spec_type + if '/' in spec_type: + # removing prefix + bare_type = (spec_type.split('/'))[1] + + msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) + c_type = msg_type + if msg_type in type_map: + c_type = type_map[msg_type] + if is_array: + return c_type + "[" + str(array_length) + "]" + return c_type + + +def print_field_def(field): + """ + Print the C type from a field + """ + type_name = field.type + # detect embedded types + sl_pos = type_name.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type_name = type_name[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type_name.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type_name[a_pos:] + type_name = type_name[:a_pos] + + if type_name in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type_name] + else: + type_px4 = type_name + + comment = '' + if field.name.startswith('_padding'): + comment = ' // required for logger' + + print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name, + array_size, comment)) diff --git a/msg/tools/uorb_rtps_message_ids.py b/msg/tools/uorb_rtps_message_ids.py new file mode 100644 index 000000000000..9d983a8cbe96 --- /dev/null +++ b/msg/tools/uorb_rtps_message_ids.py @@ -0,0 +1,108 @@ + +# Message identification constants + + +msg_id_map = { + 'actuator_armed': 1, + 'actuator_controls': 2, + 'actuator_direct': 3, + 'actuator_outputs': 4, + 'adc_report': 5, + 'airspeed': 6, + 'att_pos_mocap': 7, + 'battery_status': 8, + 'camera_capture': 9, + 'camera_trigger': 10, + 'collision_report': 11, + 'commander_state': 12, + + 'cpuload': 14, + 'debug_key_value': 15, + 'differential_pressure': 16, + 'distance_sensor': 17, + 'ekf2_innovations': 18, + + 'ekf2_timestamps': 20, + 'esc_report': 21, + 'esc_status': 22, + 'estimator_status': 23, + 'fence': 24, + 'fence_vertex': 25, + + 'follow_target': 27, + 'fw_pos_ctrl_status': 28, + 'geofence_result': 29, + 'gps_dump': 30, + 'gps_inject_data': 31, + + 'home_position': 33, + 'input_rc': 34, + 'led_control': 35, + 'log_message': 36, + 'manual_control_setpoint': 37, + 'mavlink_log': 38, + 'mc_att_ctrl_status': 39, + 'mission': 40, + 'mission_result': 41, + 'mount_orientation': 42, + 'multirotor_motor_limits': 43, + 'offboard_control_mode': 44, + 'optical_flow': 45, + + 'parameter_update': 47, + 'position_setpoint': 48, + 'position_setpoint_triplet': 49, + 'pwm_input': 50, + 'qshell_req': 51, + 'rc_channels': 52, + 'rc_parameter_map': 53, + 'safety': 54, + 'satellite_info': 55, + 'sensor_accel': 56, + 'sensor_baro': 57, + 'sensor_combined': 58, + 'sensor_correction': 59, + 'sensor_gyro': 60, + 'sensor_mag': 61, + 'sensor_preflight': 62, + 'sensor_selection': 63, + 'servorail_status': 64, + 'subsystem_info': 65, + 'system_power': 66, + 'task_stack_info': 67, + 'tecs_status': 68, + 'telemetry_status': 69, + 'test_motor': 70, + 'time_offset': 71, + 'transponder_report': 72, + 'uavcan_parameter_request': 73, + 'uavcan_parameter_value': 74, + 'ulog_stream_ack': 75, + 'ulog_stream': 76, + 'vehicle_attitude': 77, + 'vehicle_attitude_setpoint': 78, + 'vehicle_command_ack': 79, + 'vehicle_command': 80, + 'vehicle_control_mode': 81, + + 'vehicle_global_position': 83, + + 'vehicle_gps_position': 85, + 'vehicle_land_detected': 86, + 'vehicle_local_position': 87, + 'vehicle_local_position_setpoint': 88, + 'vehicle_rates_setpoint': 89, + 'vehicle_roi': 90, + 'vehicle_status_flags': 91, + 'vehicle_status': 92, + 'vtol_vehicle_status': 93, + 'wind_estimate': 94, +} + +def message_id(message): + """ + Get id of a message + """ + if message in msg_id_map: + return msg_id_map[message] + return 0 diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg new file mode 100644 index 000000000000..11a3be9e6088 --- /dev/null +++ b/msg/transponder_report.msg @@ -0,0 +1,24 @@ +uint32 ICAO_address # ICAO address +float64 lat # Latitude, expressed as degrees +float64 lon # Longitude, expressed as degrees +uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum +float32 altitude # Altitude(ASL) in meters +float32 heading # Course over ground in radians, -pi to +pi, 0 is north +float32 hor_velocity # The horizontal velocity in m/s +float32 ver_velocity # The vertical velocity in m/s, positive is up +char[9] callsign # The callsign, 8+null +uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum +uint8 tslc # Time since last communication in seconds +uint16 flags # Flags to indicate various statuses including valid data fields +uint16 squawk # Squawk code + +# ADSB flags +uint16 PX4_ADSB_FLAGS_VALID_COORDS = 1 +uint16 PX4_ADSB_FLAGS_VALID_ALTITUDE = 2 +uint16 PX4_ADSB_FLAGS_VALID_HEADING = 4 +uint16 PX4_ADSB_FLAGS_VALID_VELOCITY = 8 +uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16 +uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32 +uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256 + +uint32 ORB_QUEUE_LENGTH = 10 diff --git a/msg/tune_control.msg b/msg/tune_control.msg new file mode 100644 index 000000000000..3bb86c86458f --- /dev/null +++ b/msg/tune_control.msg @@ -0,0 +1,27 @@ +# This message is used to control the tunes, when the tune_id is set to CUSTOM +# then the frequency, duration are used otherwise those values are ignored. + +# definition of the different tune_id +uint8 TUNE_ID_CUSTOM = 0 +uint8 TUNE_ID_STARTUP = 1 +uint8 TUNE_ID_ERROR = 2 +uint8 TUNE_ID_NOTIFY_POSITIVE = 3 +uint8 TUNE_ID_NOTIFY_NEUTRAL = 4 +uint8 TUNE_ID_NOTIFY_NEGATIVE = 5 +uint8 TUNE_ID_ARMING_WARNING = 6 +uint8 TUNE_ID_BATTERY_WARNING_SLOW = 7 +uint8 TUNE_ID_BATTERY_WARNING_FAST = 8 +uint8 TUNE_ID_GPS_WARNING = 9 +uint8 TUNE_ID_ARMING_FAILURE = 10 +uint8 TUNE_ID_PARACHUTE_RELEASE = 11 +uint8 TUNE_ID_EKF_WARNING = 12 +uint8 TUNE_ID_BARO_WARNING = 13 +uint8 TUNE_ID_SINGLE_BEEP = 14 +uint8 TUNE_ID_HOME_SET = 15 + +uint8 tune_id +uint8 tune_override # if set to 1 the tune which is playing will be stopped and the new started +uint16 frequency # in Hz +uint32 duration # in us +uint32 silence # in us +uint8 strength # value between 0-100 if supported by backend diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg new file mode 100644 index 000000000000..9ced52bbaed6 --- /dev/null +++ b/msg/uavcan_parameter_request.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge request type +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/uavcan_parameter_value.msg b/msg/uavcan_parameter_value.msg new file mode 100644 index 000000000000..091c5fd278bc --- /dev/null +++ b/msg/uavcan_parameter_value.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge response type +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/ulog_stream.msg b/msg/ulog_stream.msg new file mode 100644 index 000000000000..446d8c1d709c --- /dev/null +++ b/msg/ulog_stream.msg @@ -0,0 +1,16 @@ +# Message to stream ULog data from the logger. Corresponds to the LOGGING_DATA +# mavlink message + +# flags bitmasks +uint8 FLAGS_NEED_ACK = 1 # if set, this message requires to be acked. + # Acked messages are published synchronous: a + # publisher waits for an ack before sending the + # next message + +uint8 length # length of data +uint8 first_message_offset # offset into data where first message starts. This + # can be used for recovery, when a previous message got lost +uint16 sequence # allows determine drops +uint8 flags # see FLAGS_* +uint8[249] data # ulog data + diff --git a/msg/ulog_stream_ack.msg b/msg/ulog_stream_ack.msg new file mode 100644 index 000000000000..b293f5c496ba --- /dev/null +++ b/msg/ulog_stream_ack.msg @@ -0,0 +1,7 @@ +# Ack a previously sent ulog_stream message that had +# the NEED_ACK flag set + +int32 ACK_TIMEOUT = 50 # timeout waiting for an ack until we retry to send the message [ms] +int32 ACK_MAX_TRIES = 50 # maximum amount of tries to (re-)send a message, each time waiting ACK_TIMEOUT ms + +uint16 sequence diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg new file mode 100644 index 000000000000..4504da967d25 --- /dev/null +++ b/msg/vehicle_attitude.msg @@ -0,0 +1,11 @@ +# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use + +float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s +float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s +float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s + +float32[4] q # Quaternion rotation from NED earth frame to XYZ body frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter + +# TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg new file mode 100644 index 000000000000..6a20d58211c2 --- /dev/null +++ b/msg/vehicle_attitude_setpoint.msg @@ -0,0 +1,27 @@ +int8 LANDING_GEAR_UP = 1 # landing gear up +int8 LANDING_GEAR_DOWN = -1 # landing gear down + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) +bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) + +bool apply_flaps + +float32 landing_gear + +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg new file mode 100644 index 000000000000..1ea206423ffa --- /dev/null +++ b/msg/vehicle_command.msg @@ -0,0 +1,113 @@ + +uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command +uint16 VEHICLE_CMD_CUSTOM_1 = 1 # test command +uint16 VEHICLE_CMD_CUSTOM_2 = 2 # test command +uint16 VEHICLE_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing +uint16 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| +uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92 # hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| +uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_YAW = 115 # Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_MODE = 176 # Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ +uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ +uint16 VEHICLE_CMD_DO_REPOSITION = 192 +uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 +uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| +uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203 +uint16 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION +uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3 # param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration +uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| +uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| +uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) +uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition +uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization +uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan +uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started +uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data +uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data + +uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | +uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | +uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | +uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | +uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | +uint8 VEHICLE_CMD_RESULT_ENUM_END = 6 # + +uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | +uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | +uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt | +uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5 # + +uint8 VEHICLE_ROI_NONE = 0 # No region of interest | +uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION | +uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION | +uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location | +uint8 VEHICLE_ROI_TARGET = 4 # Point toward target +uint8 VEHICLE_ROI_ENUM_END = 5 + +uint8 ORB_QUEUE_LENGTH = 3 + +float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param3 # Parameter 3, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param4 # Parameter 4, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param5 # Parameter 5, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param6 # Parameter 6, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param7 # Parameter 7, as defined by MAVLink uint16 VEHICLE_CMD enum. +uint16 command # Command ID, as defined MAVLink by uint16 VEHICLE_CMD enum. +uint8 target_system # System which should execute the command +uint8 target_component # Component which should execute the command, 0 for all components +uint8 source_system # System sending the command +uint8 source_component # Component sending the command +uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +bool from_external diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg new file mode 100644 index 000000000000..0fd93e6fa376 --- /dev/null +++ b/msg/vehicle_command_ack.msg @@ -0,0 +1,23 @@ +uint8 VEHICLE_RESULT_ACCEPTED = 0 +uint8 VEHICLE_RESULT_TEMPORARILY_REJECTED = 1 +uint8 VEHICLE_RESULT_DENIED = 2 +uint8 VEHICLE_RESULT_UNSUPPORTED = 3 +uint8 VEHICLE_RESULT_FAILED = 4 +uint8 VEHICLE_RESULT_IN_PROGRESS = 5 + +uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 +uint16 ARM_AUTH_DENIED_REASON_NONE = 1 +uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 +uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 +uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 +uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 + +uint32 ORB_QUEUE_LENGTH = 3 + +uint16 command +uint8 result +bool from_external +uint8 result_param1 +int32 result_param2 +uint8 target_system +uint8 target_component diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg new file mode 100644 index 000000000000..2a3e789a911f --- /dev/null +++ b/msg/vehicle_control_mode.msg @@ -0,0 +1,24 @@ + + # is set whenever the writing thread stores new data + +bool flag_armed + +bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing + +# XXX needs yet to be set by state machine helper +bool flag_system_hil_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled +bool flag_control_force_enabled # true if force control is mixed in +bool flag_control_acceleration_enabled # true if acceleration is controlled +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_position_enabled # true if position is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_fixed_hdg_enabled # true if using a fixed heading for user control diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg new file mode 100644 index 000000000000..707e87882ce3 --- /dev/null +++ b/msg/vehicle_global_position.msg @@ -0,0 +1,36 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# + +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) + +float32 delta_alt # Reset delta for altitude +uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 alt_reset_counter # Counter for reset events on altitude + +float32 vel_n # North velocity in NED earth-fixed frame, (metres/sec) +float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) +float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) + +float32 pos_d_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec) + +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 evh # Standard deviation of horizontal velocity error, (metres/sec) +float32 evv # Standard deviation of horizontal velocity error, (metres/sec) + +float32 terrain_alt # Terrain altitude WGS84, (metres) +bool terrain_alt_valid # Terrain altitude estimate is valid + +float32 pressure_alt # Pressure altitude AMSL, (metres) + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# TOPICS vehicle_global_position vehicle_global_position_groundtruth diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg new file mode 100644 index 000000000000..c57a07803321 --- /dev/null +++ b/msg/vehicle_gps_position.msg @@ -0,0 +1,31 @@ +# GPS position in WGS84 coordinates. +# the auto-generated field 'timestamp' is for the position & velocity (microseconds) +int32 lat # Latitude in 1E-7 degrees +int32 lon # Longitude in 1E-7 degrees +int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) +int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) + +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) + +float32 hdop # Horizontal dilution of precision +float32 vdop # Vertical dilution of precision + +int32 noise_per_ms # GPS noise per millisecond +int32 jamming_indicator # indicates jamming is occurring + +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid + +int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg new file mode 100644 index 000000000000..387d18f0ebf9 --- /dev/null +++ b/msg/vehicle_land_detected.msg @@ -0,0 +1,5 @@ +bool landed # true if vehicle is currently landed on the ground +bool freefall # true if vehicle is currently in free-fall +bool ground_contact # true if vehicle has ground contact but is not landed +bool maybe_landed # true if the vehicle might have landed +float32 alt_max # maximum altitude in [m] that can be reached diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg new file mode 100644 index 000000000000..2d8aad2160a6 --- /dev/null +++ b/msg/vehicle_local_position.msg @@ -0,0 +1,62 @@ +# Fused local position in NED. + +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) + +# Position reset delta +float32[2] delta_xy +uint8 xy_reset_counter + +float32 delta_z +uint8 z_reset_counter + +# Velocity in NED frame +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) +float32 z_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec) + +# Velocity reset delta +float32[2] delta_vxy +uint8 vxy_reset_counter + +float32 delta_vz +uint8 vz_reset_counter +# Acceleration in NED frame +float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) + +# Heading +float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) + +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) +bool z_global # true if z has a valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, MUST be set to current (not at reference point!) ground level, (metres) + +# Distance to surface +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +float32 dist_bottom_rate # Rate of change of distance from bottom surface to ground, (metres/sec) +bool dist_bottom_valid # true if distance to bottom surface is valid + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 evh # Standard deviation of horizontal velocity error, (metres/sec) +float32 evv # Standard deviation of horizontal velocity error, (metres/sec) + +# estimator specified vehicle limits +float32 vxy_max # maximum horizontal speed - set to 0 when not applicable (metres/sec) +bool limit_hagl # true when the height above ground needs to be limited to observe optical flow focus and range finder limits + +# TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_vision_position diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg new file mode 100644 index 000000000000..a5407519bb69 --- /dev/null +++ b/msg/vehicle_local_position_setpoint.msg @@ -0,0 +1,12 @@ +# Local position setpoint in NED frame + +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED +float32 yaw # in radians NED -PI..+PI +float32 vx # in meters/sec +float32 vy # in meters/sec +float32 vz # in meters/sec +float32 acc_x # in meters/(sec*sec) +float32 acc_y # in meters/(sec*sec) +float32 acc_z # in meters/(sec*sec) diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg new file mode 100644 index 000000000000..13fd4d4b6c41 --- /dev/null +++ b/msg/vehicle_rates_setpoint.msg @@ -0,0 +1,7 @@ + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 + +# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint diff --git a/msg/vehicle_roi.msg b/msg/vehicle_roi.msg new file mode 100644 index 000000000000..d1e32ddd981d --- /dev/null +++ b/msg/vehicle_roi.msg @@ -0,0 +1,21 @@ +# Vehicle Region Of Interest (ROI) + +uint8 ROI_NONE = 0 # No region of interest +uint8 ROI_WPNEXT = 1 # Point toward next MISSION with optional offset +uint8 ROI_WPINDEX = 2 # Point toward given MISSION +uint8 ROI_LOCATION = 3 # Point toward fixed location +uint8 ROI_TARGET = 4 # Point toward target +uint8 ROI_ENUM_END = 5 + +uint8 mode # ROI mode (see above) + +uint32 mission_seq # mission sequence to point to +uint32 target_seq # target sequence to point to + +float64 lat # Latitude to point to +float64 lon # Longitude to point to +float32 alt # Altitude to point to + +float32 pitchOffset # Additional pitch offset to next waypoint +float32 rollOffset # Additional roll offset to next waypoint +float32 yawOffset # Additional yaw offset to next waypoint diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg new file mode 100644 index 000000000000..1d506eb088a1 --- /dev/null +++ b/msg/vehicle_status.msg @@ -0,0 +1,72 @@ +# If you change the order, add or remove arming_state_t states make sure to update the arrays +# in state_machine_helper.cpp as well. +uint8 ARMING_STATE_INIT = 0 +uint8 ARMING_STATE_STANDBY = 1 +uint8 ARMING_STATE_ARMED = 2 +uint8 ARMING_STATE_ARMED_ERROR = 3 +uint8 ARMING_STATE_STANDBY_ERROR = 4 +uint8 ARMING_STATE_REBOOT = 5 +uint8 ARMING_STATE_IN_AIR_RESTORE = 6 +uint8 ARMING_STATE_MAX = 7 + +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# Navigation state, i.e. "what should vehicle do". +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6 # RC recover mode +uint8 NAVIGATION_STATE_AUTO_RTGS = 7 # Auto return to groundstation on data link loss +uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure +uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down) +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_MAX = 21 + +uint8 RC_IN_MODE_DEFAULT = 0 +uint8 RC_IN_MODE_OFF = 1 +uint8 RC_IN_MODE_GENERATED = 2 + +# state machine / state of vehicle. +# Encodes the complete system state and is set by the commander app. + +uint8 nav_state # set navigation state machine to specified value +uint8 arming_state # current arming state +uint8 hil_state # current hil state +bool failsafe # true if system is in failsafe state + +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool is_vtol # True if the system is VTOL capable +bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +bool rc_signal_lost # true if RC reception lost +uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. + +bool data_link_lost # datalink to GCS lost +uint8 data_link_lost_counter # counts unique data link lost events +bool engine_failure # Set to true if an engine failure is detected +bool mission_failure # Set to true if mission could not continue/finish + +# see SYS_STATUS mavlink message for the following +uint32 onboard_control_sensors_present +uint32 onboard_control_sensors_enabled +uint32 onboard_control_sensors_health diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg new file mode 100644 index 000000000000..7abfeece1232 --- /dev/null +++ b/msg/vehicle_status_flags.msg @@ -0,0 +1,35 @@ +# This is a struct used by the commander internally. + +bool condition_calibration_enabled +bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over +bool condition_system_returned_to_home +bool condition_auto_mission_available +bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation +bool condition_global_velocity_valid # set to true by the commander app if the quality of the global horizontal velocity data is good enough to use for navigation +bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation +bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation +bool condition_local_altitude_valid +bool condition_airspeed_valid # set to true by the commander app if there is a valid airspeed measurement available +bool condition_power_input_valid # set if input power is valid + +bool circuit_breaker_engaged_power_check +bool circuit_breaker_engaged_airspd_check +bool circuit_breaker_engaged_enginefailure_check +bool circuit_breaker_engaged_gpsfailure_check +bool circuit_breaker_flight_termination_disabled +bool circuit_breaker_engaged_usb_check +bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled + +bool offboard_control_signal_found_once +bool offboard_control_signal_lost +bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC +bool offboard_control_loss_timeout # true if offboard is lost for a certain amount of time + +bool rc_signal_found_once +bool rc_input_blocked # set if RC input should be ignored temporarily +bool vtol_transition_failure # Set to true if vtol transition failed +bool gps_failure # Set to true if a gps failure is detected +bool usb_connected # status of the USB power supply diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg new file mode 100644 index 000000000000..1efece610fa4 --- /dev/null +++ b/msg/vtol_vehicle_status.msg @@ -0,0 +1,12 @@ +# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE +uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VEHICLE_VTOL_STATE_MC = 3 +uint8 VEHICLE_VTOL_STATE_FW = 4 + +bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode +bool vtol_in_trans_mode +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW +bool vtol_transition_failsafe # vtol in transition failsafe mode +bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg new file mode 100644 index 000000000000..b8e47a88752f --- /dev/null +++ b/msg/wind_estimate.msg @@ -0,0 +1,5 @@ +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated diff --git a/platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig b/platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig index 9780b9c56cd0..844f4dd66ec7 100644 --- a/platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig +++ b/platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig @@ -526,18 +526,6 @@ CONFIG_STM32_USART2_SERIALDRIVER=y # CONFIG_STM32_USART2_1WIREDRIVER is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y -<<<<<<< HEAD:platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig -||||||| merged common ancestors -CONFIG_STM32_USART3_SERIALDRIVER=y -# CONFIG_STM32_USART3_1WIREDRIVER is not set -# CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y -======= -# CONFIG_STM32_USART3_SERIALDRIVER is not set -# CONFIG_STM32_USART3_1WIREDRIVER is not set -# CONFIG_USART3_RS485 is not set -# CONFIG_USART3_RXDMA is not set ->>>>>>> gamma_rigid_wing:nuttx-configs/aerofc-v1/nsh/defconfig CONFIG_STM32_UART4_SERIALDRIVER=y # CONFIG_STM32_UART4_1WIREDRIVER is not set # CONFIG_UART4_RS485 is not set @@ -550,7 +538,6 @@ CONFIG_STM32_USART6_SERIALDRIVER=y # CONFIG_STM32_USART6_1WIREDRIVER is not set # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y -<<<<<<< HEAD:platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig CONFIG_STM32_UART7_SERIALDRIVER=y # CONFIG_STM32_UART7_1WIREDRIVER is not set # CONFIG_UART7_RS485 is not set @@ -559,13 +546,6 @@ CONFIG_UART7_RXDMA=y # CONFIG_STM32_UART8_1WIREDRIVER is not set # CONFIG_UART8_RS485 is not set # CONFIG_UART8_RXDMA is not set -||||||| merged common ancestors -======= -CONFIG_STM32_UART7_SERIALDRIVER=y -# CONFIG_STM32_UART7_1WIREDRIVER is not set -# CONFIG_UART7_RS485 is not set -CONFIG_UART7_RXDMA=y ->>>>>>> gamma_rigid_wing:nuttx-configs/aerofc-v1/nsh/defconfig # # Serial Driver Configuration @@ -994,13 +974,8 @@ CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set CONFIG_USART6_SERIAL_CONSOLE=y -<<<<<<< HEAD:platforms/nuttx/nuttx-configs/aerofc-v1/nsh/defconfig # CONFIG_UART7_SERIAL_CONSOLE is not set # CONFIG_UART8_SERIAL_CONSOLE is not set -||||||| merged common ancestors -======= -# CONFIG_UART7_SERIAL_CONSOLE is not set ->>>>>>> gamma_rigid_wing:nuttx-configs/aerofc-v1/nsh/defconfig # CONFIG_OTHER_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set @@ -1102,19 +1077,6 @@ CONFIG_UART8_2STOP=0 # CONFIG_DRIVERS_WIRELESS is not set # CONFIG_DRIVERS_CONTACTLESS is not set -# -# UART7 Configuration -# -CONFIG_UART7_RXBUFSIZE=300 -CONFIG_UART7_TXBUFSIZE=300 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_BITS=8 -CONFIG_UART7_PARITY=0 -CONFIG_UART7_2STOP=0 -# CONFIG_UART7_IFLOWCONTROL is not set -# CONFIG_UART7_OFLOWCONTROL is not set -# CONFIG_UART7_DMA is not set - # # System Logging # diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index f81873dea523..9cb9ab4463d1 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -47,13 +47,7 @@ ExternalProject_Add_Step(sitl_gazebo forceconfigure # create targets for each viewer/model/debugger combination set(viewers none jmavsim gazebo replay) set(debuggers none ide gdb lldb ddd valgrind callgrind) -<<<<<<< HEAD:platforms/posix/cmake/sitl_target.cmake set(models none iris iris_opt_flow iris_rplidar iris_irlock standard_vtol plane solo tailsitter typhoon_h480 rover hippocampus) -||||||| merged common ancestors -set(models none iris iris_opt_flow iris_rplidar standard_vtol plane solo tailsitter typhoon_h480 rover) -======= -set(models none iris iris_opt_flow iris_rplidar standard_vtol plane solo tailsitter typhoon_h480 rover rigid_wing) ->>>>>>> gamma_rigid_wing:src/firmware/posix/sitl_target.cmake set(all_posix_vmd_make_targets) foreach(viewer ${viewers}) foreach(debugger ${debuggers}) diff --git a/posix-configs/SITL/init/ekf2/rigid_wing b/posix-configs/SITL/init/ekf2/rigid_wing deleted file mode 100644 index 445e1705edf4..000000000000 --- a/posix-configs/SITL/init/ekf2/rigid_wing +++ /dev/null @@ -1,96 +0,0 @@ -uorb start -param load -dataman start -param set BAT_N_CELLS 3 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_ID 1310728 -param set CAL_ACC1_XOFF 0.01 -param set CAL_GYRO0_ID 2293768 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_MAG0_ID 196616 -param set CAL_MAG0_XOFF 0.01 -param set COM_DISARM_LAND 5 -param set COM_RC_IN_MODE 1 -param set EKF2_AID_MASK 1 -param set EKF2_ANGERR_INIT 0.01 -param set EKF2_GBIAS_INIT 0.01 -param set EKF2_HGT_MODE 0 -param set EKF2_MAG_TYPE 1 -param set FW_AIRSPD_MAX 25 -param set FW_AIRSPD_MIN 14 -param set FW_AIRSPD_TRIM 16 -param set MAV_TYPE 22 -param set MC_PITCH_P 6 -param set MC_PITCHRATE_P 0.2 -param set MC_ROLL_P 6 -param set MC_ROLLRATE_P 0.3 -param set MIS_LTRMIN_ALT 10 -param set MIS_TAKEOFF_ALT 10 -param set MIS_YAW_TMT 10 -param set MPC_ACC_HOR_MAX 2 -param set MPC_ACC_HOR_MAX 2.0 -param set MPC_TKO_SPEED 1.0 -param set MPC_XY_P 0.8 -param set MPC_XY_VEL_D 0.005 -param set MPC_XY_VEL_I 0.2 -param set MPC_XY_VEL_P 0.15 -param set MPC_Z_VEL_I 0.15 -param set MPC_Z_VEL_MAX_DN 1.5 -param set MPC_Z_VEL_P 0.6 -param set NAV_ACC_RAD 5.0 -param set NAV_DLL_ACT 2 -param set NAV_LOITER_RAD 80 -param set RTL_DESCEND_ALT 10.0 -param set RTL_LAND_DELAY 0 -param set RTL_RETURN_ALT 30.0 -param set SENS_BOARD_ROT 0 -param set SENS_BOARD_X_OFF 0.000001 -param set SENS_DPRES_OFF 0.001 -param set SYS_AUTOSTART 13014 -param set SYS_MC_EST_GROUP 2 -param set SYS_RESTART_TYPE 2 -param set VT_MOT_COUNT 4 -param set VT_TRANS_THR 0.75 -param set VT_TYPE 3 -replay tryapplyparams -simulator start -s -tone_alarm start -gyrosim start -accelsim start -barosim start -adcsim start -gpssim start -measairspeedsim start -pwm_out_sim mode_pwm -sensors start -commander start -land_detector start multicopter -navigator start -ekf2 start -vtol_att_control start -mc_pos_control start -mc_att_control start -fw_pos_control_l1 start -fw_att_control start -mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/kitepower_aeolus250.main.mix -mavlink start -u 14556 -r 2000000 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557 -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 80 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 -mavlink stream -r 20 -s RC_CHANNELS -u 14556 -mavlink stream -r 250 -s HIGHRES_IMU -u 14556 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -logger start -e -t -mavlink boot_complete -replay trystart diff --git a/src/drivers/aerofc_adc/aerofc_adc.cpp b/src/drivers/aerofc_adc/aerofc_adc.cpp index 09f16ca20dc7..a3cf0dc92541 100644 --- a/src/drivers/aerofc_adc/aerofc_adc.cpp +++ b/src/drivers/aerofc_adc/aerofc_adc.cpp @@ -175,23 +175,8 @@ int aerofc_adc_main(int argc, char *argv[]) continue; } -<<<<<<< HEAD instance = new AEROFC_ADC(bus_options[i].busnum); -||||||| merged common ancestors - if (!instance) { - warn("No memory to instance AEROFC_ADC"); - return PX4_ERROR; - } -======= - instance = new AEROFC_ADC(bus_options[i].busnum); - - if (!instance) { - warn("No memory to instance AEROFC_ADC"); - return PX4_ERROR; - } ->>>>>>> gamma_rigid_wing -<<<<<<< HEAD if (!instance) { warn("No memory to instance AEROFC_ADC"); return PX4_ERROR; @@ -202,15 +187,6 @@ int aerofc_adc_main(int argc, char *argv[]) } warn("AEROFC_ADC not found on busnum=%u", bus_options[i].busnum); -||||||| merged common ancestors - if (instance->init() != PX4_OK) { -======= - if (instance->init() == PX4_OK) { - break; - } - - warn("AEROFC_ADC not found on busnum=%u", bus_options[i].busnum); ->>>>>>> gamma_rigid_wing delete instance; instance = nullptr; } diff --git a/src/drivers/boards/aerofc-v1/aerofc_init.c b/src/drivers/boards/aerofc-v1/aerofc_init.c index 4b82b8291e68..0bc8e8b83e1d 100644 --- a/src/drivers/boards/aerofc-v1/aerofc_init.c +++ b/src/drivers/boards/aerofc-v1/aerofc_init.c @@ -116,7 +116,6 @@ extern void led_off(int led); __END_DECLS /**************************************************************************** -<<<<<<< HEAD * Private Functions ****************************************************************************/ @@ -130,22 +129,6 @@ static int _bootloader_force_pin_callback(int irq, void *context, void *args) } /**************************************************************************** -||||||| merged common ancestors -======= - * Private Functions - ****************************************************************************/ - -static int _bootloader_force_pin_callback(int irq, void *context) -{ - if (stm32_gpioread(GPIO_FORCE_BOOTLOADER)) { - board_reset(0); - } - - return 0; -} - -/**************************************************************************** ->>>>>>> gamma_rigid_wing * Protected Functions ****************************************************************************/ @@ -165,15 +148,8 @@ static int _bootloader_force_pin_callback(int irq, void *context) __EXPORT void stm32_boardinitialize(void) { -<<<<<<< HEAD stm32_configgpio(GPIO_FORCE_BOOTLOADER); _bootloader_force_pin_callback(0, NULL, NULL); -||||||| merged common ancestors - /* configure LEDs */ -======= - stm32_configgpio(GPIO_FORCE_BOOTLOADER); - _bootloader_force_pin_callback(0, NULL); ->>>>>>> gamma_rigid_wing /* configure LEDs */ board_autoled_initialize(); @@ -198,16 +174,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) { int result; -<<<<<<< HEAD /* the interruption subsystem is not initialized when stm32_boardinitialize() is called */ stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback, NULL); -||||||| merged common ancestors -======= - /* the interruption subsystem is not initialized when stm32_boardinitialize() is called */ - stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback); - ->>>>>>> gamma_rigid_wing #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) /* run C++ ctors before we go any further */ diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp deleted file mode 100644 index b33e18946265..000000000000 --- a/src/drivers/ll40ls/ll40ls.cpp +++ /dev/null @@ -1,421 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ll40ls.cpp - * @author Allyson Kreft - * @author Johan Jansen - * @author Ban Siesta - * @author James Goppert - * - * Interface for the PulsedLight Lidar-Lite range finders. - */ - -#include "LidarLiteI2C.h" -#include "LidarLitePWM.h" -#include -#include -#include -#include -#include -#include - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" - -enum LL40LS_BUS { - LL40LS_BUS_I2C_ALL = 0, - LL40LS_BUS_I2C_INTERNAL, - LL40LS_BUS_I2C_EXTERNAL, - LL40LS_BUS_PWM -}; - -static struct ll40ls_bus_option { - enum LL40LS_BUS busid; - const char *devname; - uint8_t busnum; -} bus_options[] = { -#ifdef PX4_I2C_BUS_EXPANSION - { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext", PX4_I2C_BUS_EXPANSION }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION1 - { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext1", PX4_I2C_BUS_EXPANSION1 }, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { LL40LS_BUS_I2C_INTERNAL, "/dev/ll40ls_int", PX4_I2C_BUS_ONBOARD }, -#endif -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); - - -/** - * Local functions in support of the shell command. - */ -namespace ll40ls -{ - -LidarLite *instance = nullptr; - -void start(enum LL40LS_BUS busid); -void stop(); -void test(); -void reset(); -void info(); -void regdump(); -void usage(); - -/** - * Start the driver. - */ -void start(enum LL40LS_BUS busid) -{ - int fd, ret; - - if (instance) { - warnx("driver already started"); - } - - if (busid == LL40LS_BUS_PWM) { - instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); - - if (!instance) { - warnx("Failed to instantiate LidarLitePWM"); - return; - } - - if (instance->init() != PX4_OK) { - warnx("failed to initialize LidarLitePWM"); - goto fail; - } - - } else { - for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) { - if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) { - continue; - } - - instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname); - - if (!instance) { - warnx("Failed to instantiate LidarLiteI2C"); - return; - } - - if (instance->init() == PX4_OK) { - break; - } - - warnx("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); - delete instance; - instance = nullptr; - } - } - - if (!instance) { - warnx("No LidarLite found"); - return; - } - - fd = open(instance->get_dev_name(), O_RDONLY); - - if (fd == -1) { - warnx("Error opening fd"); - goto fail; - } - - ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); - - if (ret < 0) { - warnx("pollrate fail"); - goto fail; - } - - return; - -fail: - delete instance; - instance = nullptr; -} - -/** - * Stop the driver - */ -void stop() -{ - delete instance; - instance = nullptr; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct distance_sensor_s report; - ssize_t sz; - int ret; - - if (!instance) { - warnx("No ll40ls driver running"); - errx(1, "FAIL"); - } - - int fd = open(instance->get_dev_name(), O_RDONLY); - - if (fd < 0) { - warnx("Error opening fd"); - errx(1, "FAIL"); - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warnx("immediate read failed"); - goto error; - } - - warnx("single read"); - warnx("measurement: %0.2f m", (double)report.current_distance); - warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - warnx("failed to set 2Hz poll rate"); - goto error; - } - - /* read the sensor 5 times and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - warnx("timed out waiting for sensor data"); - goto error; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warnx("periodic read failed"); - goto error; - } - - warnx("periodic read %u", i); - warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); - warnx("measurement: %0.3f m", (double)report.current_distance); - warnx("time: %lld", report.timestamp); - } - - /* reset the sensor polling to default rate */ - if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - warnx("failed to set default poll rate"); - goto error; - } - - close(fd); - errx(0, "PASS"); - -error: - close(fd); - errx(1, "FAIL"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - if (!instance) { - warnx("No ll40ls driver running"); - return; - } - - int fd = open(instance->get_dev_name(), O_RDONLY); - - if (fd < 0) { - warnx("Error opening fd"); - return; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - warnx("driver reset failed"); - goto error; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("driver poll restart failed"); - goto error; - } - -error: - close(fd); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (!instance) { - warnx("No ll40ls driver running"); - return; - } - - printf("state @ %p\n", instance); - instance->print_info(); -} - -/** - * Dump registers - */ -void -regdump() -{ - if (!instance) { - warnx("No ll40ls driver running"); - return; - } - - printf("regdump @ %p\n", instance); - instance->print_registers(); -} - -void -usage() -{ - warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); - warnx("options for I2C:"); - warnx(" -X only external bus"); -#ifdef PX4_I2C_BUS_ONBOARD - warnx(" -I only internal bus"); -#endif -} - -} // namespace - -int -ll40ls_main(int argc, char *argv[]) -{ - int ch; - enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL; - - while ((ch = getopt(argc, argv, "XI")) != EOF) { - switch (ch) { -#ifdef PX4_I2C_BUS_ONBOARD - - case 'I': - busid = LL40LS_BUS_I2C_INTERNAL; - break; -#endif - - case 'X': - busid = LL40LS_BUS_I2C_EXTERNAL; - break; - - default: - ll40ls::usage(); - return 0; - } - } - - /* determine protocol first because it's needed next */ - if (argc > optind + 1) { - const char *protocol = argv[optind + 1]; - - if (!strcmp(protocol, "pwm")) { - busid = LL40LS_BUS_PWM;; - - } else if (!strcmp(protocol, "i2c")) { - // Do nothing - - } else { - warnx("unknown protocol, choose pwm or i2c"); - ll40ls::usage(); - return 0; - } - } - - /* now determine action */ - if (argc > optind) { - const char *verb = argv[optind]; - - if (!strcmp(verb, "start")) { - ll40ls::start(busid); - - } else if (!strcmp(verb, "stop")) { - ll40ls::stop(); - - } else if (!strcmp(verb, "test")) { - ll40ls::test(); - - } else if (!strcmp(verb, "reset")) { - ll40ls::reset(); - - } else if (!strcmp(verb, "regdump")) { - ll40ls::regdump(); - - } else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - ll40ls::info(); - - } else { - ll40ls::usage(); - } - - return 0; - } - - warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); - ll40ls::usage(); - return 0; -} diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 8853afe5dad2..8bd087d9a29d 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -130,7 +130,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl debug("remaining in buf: %d, first char: %c", buflen, buf[0]); -<<<<<<< HEAD:src/lib/mixer/mixer_multirotor.cpp for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY; i++) { if (!strcmp(geomname, _config_key[i])) { @@ -138,132 +137,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl break; } } -||||||| merged common ancestors - if (!strcmp(geomname, "4+")) { - geometry = MultirotorGeometry::QUAD_PLUS; - - } else if (!strcmp(geomname, "4x")) { - geometry = MultirotorGeometry::QUAD_X; - - } else if (!strcmp(geomname, "4h")) { - geometry = MultirotorGeometry::QUAD_H; - - } else if (!strcmp(geomname, "4v")) { - geometry = MultirotorGeometry::QUAD_V; - - } else if (!strcmp(geomname, "4w")) { - geometry = MultirotorGeometry::QUAD_WIDE; - - } else if (!strcmp(geomname, "4s")) { - geometry = MultirotorGeometry::QUAD_S250AQ; - - } else if (!strcmp(geomname, "4dc")) { - geometry = MultirotorGeometry::QUAD_DEADCAT; - - } else if (!strcmp(geomname, "6+")) { - geometry = MultirotorGeometry::HEX_PLUS; - - } else if (!strcmp(geomname, "6x")) { - geometry = MultirotorGeometry::HEX_X; - - } else if (!strcmp(geomname, "6c")) { - geometry = MultirotorGeometry::HEX_COX; - - } else if (!strcmp(geomname, "6t")) { - geometry = MultirotorGeometry::HEX_T; - - } else if (!strcmp(geomname, "8+")) { - geometry = MultirotorGeometry::OCTA_PLUS; - - } else if (!strcmp(geomname, "8x")) { - geometry = MultirotorGeometry::OCTA_X; - - } else if (!strcmp(geomname, "8c")) { - geometry = MultirotorGeometry::OCTA_COX; - - } else if (!strcmp(geomname, "6m")) { - geometry = MultirotorGeometry::DODECA_TOP_COX; - - } else if (!strcmp(geomname, "6a")) { - geometry = MultirotorGeometry::DODECA_BOTTOM_COX; - - -#if 0 - - } else if (!strcmp(geomname, "8cw")) { - geometry = MultirotorGeometry::OCTA_COX_WIDE; -#endif - - } else if (!strcmp(geomname, "2-")) { - geometry = MultirotorGeometry::TWIN_ENGINE; - - } else if (!strcmp(geomname, "3y")) { - geometry = MultirotorGeometry::TRI_Y; -======= - if (!strcmp(geomname, "4+")) { - geometry = MultirotorGeometry::QUAD_PLUS; - - } else if (!strcmp(geomname, "4x")) { - geometry = MultirotorGeometry::QUAD_X; - - } else if (!strcmp(geomname, "4aeol")){ - geometry = MultirotorGeometry::KITEPOWER_AEOLUS250; - - } else if (!strcmp(geomname, "4h")) { - geometry = MultirotorGeometry::QUAD_H; - - } else if (!strcmp(geomname, "4v")) { - geometry = MultirotorGeometry::QUAD_V; - - } else if (!strcmp(geomname, "4w")) { - geometry = MultirotorGeometry::QUAD_WIDE; - - } else if (!strcmp(geomname, "4s")) { - geometry = MultirotorGeometry::QUAD_S250AQ; - - } else if (!strcmp(geomname, "4dc")) { - geometry = MultirotorGeometry::QUAD_DEADCAT; - - } else if (!strcmp(geomname, "6+")) { - geometry = MultirotorGeometry::HEX_PLUS; - - } else if (!strcmp(geomname, "6x")) { - geometry = MultirotorGeometry::HEX_X; - - } else if (!strcmp(geomname, "6c")) { - geometry = MultirotorGeometry::HEX_COX; - - } else if (!strcmp(geomname, "6t")) { - geometry = MultirotorGeometry::HEX_T; - - } else if (!strcmp(geomname, "8+")) { - geometry = MultirotorGeometry::OCTA_PLUS; - - } else if (!strcmp(geomname, "8x")) { - geometry = MultirotorGeometry::OCTA_X; - - } else if (!strcmp(geomname, "8c")) { - geometry = MultirotorGeometry::OCTA_COX; - - } else if (!strcmp(geomname, "6m")) { - geometry = MultirotorGeometry::DODECA_TOP_COX; - - } else if (!strcmp(geomname, "6a")) { - geometry = MultirotorGeometry::DODECA_BOTTOM_COX; - - -#if 0 - - } else if (!strcmp(geomname, "8cw")) { - geometry = MultirotorGeometry::OCTA_COX_WIDE; -#endif - - } else if (!strcmp(geomname, "2-")) { - geometry = MultirotorGeometry::TWIN_ENGINE; - - } else if (!strcmp(geomname, "3y")) { - geometry = MultirotorGeometry::TRI_Y; ->>>>>>> gamma_rigid_wing:src/modules/systemlib/mixer/mixer_multirotor.cpp if (geometry == MultirotorGeometry::MAX_GEOMETRY) { debug("unrecognised geometry '%s'", geomname); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2407f7e03ea..90db3fd2da4a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2588,24 +2588,19 @@ Commander::run() _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; - if ((in_armed_state && + if (in_armed_state && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && - (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) - || (in_armed_state && (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON)) // Add rule for killswitch - ) { + (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition) ) { if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && internal_state.main_state != commander_state_s::MAIN_STATE_STAB && - internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)// && - //!land_detector.landed) // Remove landed condition - { + internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && + !land_detector.landed) { print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); - } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition - || sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON // Add rule for killswitch - ) { + } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -2732,24 +2727,6 @@ Commander::run() status.rc_signal_lost = true; rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; - // Add disarm code - /* disarm for lockdown */ - //transition_result_t arming_ret; - arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : - vehicle_status_s::ARMING_STATE_STANDBY_ERROR); - //arming_ret = - arming_state_transition(&status, - &battery, - &safety, - new_arming_state, - &armed, - true /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, - hrt_elapsed_time(&commander_boot_timestamp)); } } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 27694f7461cc..beff42bc9b82 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -804,7 +804,7 @@ FixedwingAttitudeControl::task_main() _pitch = euler_angles(1); _yaw = euler_angles(2); - if (_vehicle_status.is_vtol && (_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER)) { + if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4222df0fd38a..3ddf0789b921 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -635,56 +635,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder _att_sp.apply_flaps = false; // by default we don't use flaps -<<<<<<< HEAD -||||||| merged common ancestors - /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_ctrl_state.x_acc, _ctrl_state.y_acc, _ctrl_state.z_acc); - - // tailsitters use the multicopter frame as reference, in fixed wing - // we need to use the fixed wing frame - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - float tmp = accel_body(0); - accel_body(0) = -accel_body(2); - accel_body(2) = tmp; - } - - math::Vector<3> accel_earth{_R_nb * accel_body}; - - /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_vehicle_land_detected.landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); - - /* update TECS filters */ - _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); - -======= - /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_ctrl_state.x_acc, _ctrl_state.y_acc, _ctrl_state.z_acc); - - // tailsitters use the multicopter frame as reference, in fixed wing - // we need to use the fixed wing frame - if ((_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER) && _vehicle_status.is_vtol) { - float tmp = accel_body(0); - accel_body(0) = -accel_body(2); - accel_body(2) = tmp; - } - - math::Vector<3> accel_earth{_R_nb * accel_body}; - - /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_vehicle_land_detected.landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); - - /* update TECS filters */ - _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); - ->>>>>>> gamma_rigid_wing calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); // l1 navigation logic breaks down when wind speed exceeds max airspeed @@ -1807,7 +1757,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight - if ((_parameters.vtol_type == vtol_type::TAILSITTER || _parameters.vtol_type == vtol_type::KITEPOWER) && _vehicle_status.is_vtol) { + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { math::Matrix<3, 3> R_offset; R_offset.from_euler(0, M_PI_2_F, 0); math::Matrix<3, 3> R_fixed_wing = _R_nb * R_offset; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py deleted file mode 100755 index 313b75c8f35e..000000000000 --- a/src/modules/systemlib/mixer/multi_tables.py +++ /dev/null @@ -1,278 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Generate multirotor mixer scale tables compatible with the ArduCopter layout -# - -# for python2.7 compatibility -from __future__ import print_function - -import math - -print("/*") -print("* This file is automatically generated by multi_tables - do not edit.") -print("*/") -print("") -print("#ifndef _MIXER_MULTI_TABLES") -print("#define _MIXER_MULTI_TABLES") -print("") - -def rcos(angleInRadians): - return math.cos(math.radians(angleInRadians)) - -CCW = 1.0 -CW = -CCW - -quad_x = [ - [ 45, CCW], - [-135, CCW], - [-45, CW], - [135, CW], -] - -#just defines motor position, order doesn't matter -kitepower_aeolus250 = [ - [ 34, CCW], - [-146, CCW], - [ -34, CW], - [ 146, CW], -] - -quad_h = [ - [ 45, CW], - [-135, CW], - [-45, CCW], - [135, CCW], -] - -quad_plus = [ - [ 90, CCW], - [ -90, CCW], - [ 0, CW], - [ 180, CW], -] - -quad_deadcat = [ - [ 60, CCW, 1.0], - [-125, CCW, 0.92], - [ -60, CW, 1.0], - [ 125, CW, 0.92], -] - -quad_v = [ - [ 18.8, 0.4242], - [ -18.8, 1.0], - [ -18.8, -0.4242], - [ 18.8, -1.0], -] - -quad_wide = [ - [ 68, CCW], - [ -129, CCW], - [ -68, CW], - [ 129, CW], -] - -quad_s250aq = [ - [ 59, CCW, 1.0 ], - [ -139, CCW, 0.67], - [ -59, CW, 1.0 ], - [ 139, CW, 0.67], -] - -hex_x = [ - [ 90, CW], - [ -90, CCW], - [ -30, CW], - [ 150, CCW], - [ 30, CCW], - [-150, CW], -] - -hex_plus = [ - [ 0, CW], - [ 180, CCW], - [-120, CW], - [ 60, CCW], - [ -60, CCW], - [ 120, CW], -] - -hex_cox = [ - [ 60, CW], - [ 60, CCW], - [ 180, CW], - [ 180, CCW], - [ -60, CW], - [ -60, CCW], -] - -hex_t = [ - [ 43.21, CCW], - [ 43.21, CW], - [ 180, CW], - [ 180, CCW], - [ -43.21, CW], - [ -43.21, CCW], -] - -octa_x = [ - [ 22.5, CW], - [-157.5, CW], - [ 67.5, CCW], - [ 157.5, CCW], - [ -22.5, CCW], - [-112.5, CCW], - [ -67.5, CW], - [ 112.5, CW], -] - -octa_plus = [ - [ 0, CW], - [ 180, CW], - [ 45, CCW], - [ 135, CCW], - [ -45, CCW], - [-135, CCW], - [ -90, CW], - [ 90, CW], -] - -octa_cox = [ - [ 45, CCW], - [ -45, CW], - [-135, CCW], - [ 135, CW], - [ -45, CCW], - [ 45, CW], - [ 135, CCW], - [-135, CW], -] - -octa_cox_wide = [ - [ 68, CCW], - [ -68, CW], - [-129, CCW], - [ 129, CW], - [ -68, CCW], - [ 68, CW], - [ 129, CCW], - [-129, CW], -] - -twin_engine = [ - [ 90, 0.0], - [-90, 0.0], -] - -tri_y = [ - [ 60, 0.0], - [ -60, 0.0], - [ 180, 0.0], -] - -dodeca_top_cox = [ - [ 90, CW], - [ -90, CCW], - [ -30, CW], - [ 150, CCW], - [ 30, CCW], - [-150, CW], -] - -dodeca_bottom_cox = [ - [ 90, CCW], - [ -90, CW], - [ -30, CCW], - [ 150, CW], - [ 30, CW], - [-150, CCW], -] - -tables = [quad_x, kitepower_aeolus250, quad_h, quad_plus, quad_v, quad_wide, quad_s250aq, quad_deadcat, hex_x, hex_plus, hex_cox, hex_t, octa_x, octa_plus, octa_cox, octa_cox_wide, twin_engine, tri_y, dodeca_top_cox, dodeca_bottom_cox] - -def variableName(variable): - for variableName, value in list(globals().items()): - if value is variable: - return variableName - -def unpackScales(scalesList): - if len(scalesList) == 2: - scalesList += [1.0] #Add thrust scale - return scalesList - -def printEnum(): - print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {") - for table in tables: - print("\t{},".format(variableName(table).upper())) - - print("\n\tMAX_GEOMETRY") - print("}; // enum class MultirotorGeometry\n") - -def printScaleTables(): - for table in tables: - print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))) - for row in table: - angle, yawScale, thrustScale = unpackScales(row) - rollScale = rcos(angle + 90) - pitchScale = rcos(angle) - print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale)) - print("};\n") - -def printScaleTablesIndex(): - print("const MultirotorMixer::Rotor *_config_index[] = {") - for table in tables: - print("\t&_config_{}[0],".format(variableName(table))) - print("};\n") - - -def printScaleTablesCounts(): - print("const unsigned _config_rotor_count[] = {") - for table in tables: - print("\t{}, /* {} */".format(len(table), variableName(table))) - print("};\n") - - - -printEnum() - -print("namespace {") -printScaleTables() -printScaleTablesIndex() -printScaleTablesCounts() - -print("} // anonymous namespace\n") -print("#endif /* _MIXER_MULTI_TABLES */") -print("") diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 4acf9b07c9d2..17d8dd242e68 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -41,7 +41,6 @@ px4_add_module( vtol_type.cpp tailsitter.cpp standard.cpp - kitepower.cpp DEPENDS platforms__common ) diff --git a/src/modules/vtol_att_control/kitepower.cpp b/src/modules/vtol_att_control/kitepower.cpp deleted file mode 100644 index fa67f25f9e96..000000000000 --- a/src/modules/vtol_att_control/kitepower.cpp +++ /dev/null @@ -1,377 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** -* @file kitepower.cpp -* -* @author Roman Babst -* @author Gabriel Koenig -* -*/ - -#include "kitepower.h" -#include "vtol_att_control_main.h" - - -Kitepower::Kitepower(VtolAttitudeControl *attc) : - VtolType(attc), - _airspeed_tot(0.0f), - _flag_enable_mc_motors(true), - _thrust_transition_start(0.0f), - _yaw_transition(0.0f), - _pitch_transition_start(0.0f), - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-kitepower")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-kitepower nonfinite input")) -{ - _vtol_schedule.flight_mode = MC_MODE; - _vtol_schedule.transition_start = 0; - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; - _mc_throttle_weight = 1.0f; - - _flag_was_in_trans_mode = false; - - -} - -Kitepower::~Kitepower() -{ - -} - -void -Kitepower::parameters_update() -{ - -} - -void Kitepower::update_vtol_state() -{ - - /* simple logic using a two way switch to perform transitions. - * after flipping the switch the vehicle will start tilting in MC control mode, picking up - * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. - * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. - */ - - //matrix::Eulerf euler = matrix::Quatf(_v_att->q); - // Kitepower: not needed at the moment - //float pitch = euler.theta(); - - if (!_attc->is_fixed_wing_requested()) { - - - switch (_vtol_schedule.flight_mode) { // user switchig to MC mode - case MC_MODE: - break; - - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - //_flag_enable_mc_motors = true; - break; - - case TRANSITION_FRONT: - // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; - //_flag_enable_mc_motors = true; - break; - - - case TRANSITION_BACK: - - //Kitepower: direct switch for the moment for transition - _vtol_schedule.flight_mode = MC_MODE; - //_flag_enable_mc_motors = true; - break; - } - - } else { // user switchig to FW mode - - switch (_vtol_schedule.flight_mode) { - case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT; - _vtol_schedule.transition_start = hrt_absolute_time(); - //_flag_enable_mc_motors = false; - break; - - case FW_MODE: - //_flag_enable_mc_motors = false; - break; - - case TRANSITION_FRONT: - //Kitpower: direct switch for the moment for front transition - _vtol_schedule.flight_mode = FW_MODE; - //_flag_enable_mc_motors = false; - break; - - case TRANSITION_BACK: - // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; - //_flag_enable_mc_motors = false; - - break; - } - } - update_external_VTOL_state(); -} -void Kitepower::update_external_VTOL_state() -{ - // map kitepower specific control phases to simple control modes - switch (_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - - case FW_MODE: - _vtol_mode = FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - - case TRANSITION_FRONT: - _vtol_mode = TRANSITION_TO_FW; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; - - case TRANSITION_BACK: - _vtol_mode = TRANSITION_TO_MC; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; - } -} - -void Kitepower::update_transition_state() -{ - if (!_flag_was_in_trans_mode) { - // save desired heading for transition and last thrust value - _yaw_transition = _v_att_sp->yaw_body; - _pitch_transition_start = _v_att_sp->pitch_body; - _thrust_transition_start = _v_att_sp->thrust; - _flag_was_in_trans_mode = true; - } - - if (_vtol_schedule.flight_mode == TRANSITION_FRONT) { - // Kitepower: at the moment empty as we have direct switch to fixed wing - // create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - - // create time dependant throttle signal higher than in MC and growing untillspeed reached */ - - // eventually disable mc yaw control once the plane has picked up speed - - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - - // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - - // throttle value is decreesed - - // smoothly move control weight to MC */ - - } - - - - // compute desired attitude and thrust setpoint for the transition - // Kitepower: at the moment we just use the same values at the beginning of the transition - - _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0.0f; - _v_att_sp->yaw_body = _yaw_transition; - _v_att_sp->pitch_body = _pitch_transition_start; - _v_att_sp->thrust = _thrust_transition_start; - - math::Quaternion q_sp; - q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); -} - -void Kitepower::waiting_on_tecs() -{ - // copy the last trust value from the front transition - _v_att_sp->thrust = _thrust_transition; -} - -void Kitepower::calc_tot_airspeed() -{ - float airspeed = math::max(1.0f, _airspeed->indicated_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; - P = math::constrain(P, 1.0f, _params->power_max); - // calculate prop efficiency - float power_factor = 1.0f - P * _params->prop_eff / _params->power_max; - float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f; - eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed / eta - airspeed) * 2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - -void Kitepower::scale_mc_output() -{ - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !PX4_ISFINITE(_airspeed->indicated_airspeed_m_s) || - hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { - airspeed = _params->mc_airspeed_trim; - - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); - } - - _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : - airspeed); - _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, - -1.0f, 1.0f); -} - -void Kitepower::update_mc_state() -{ - VtolType::update_mc_state(); - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; - _mc_throttle_weight = 1.0f; - - /*for the moment let the motor spin during fixed wing, nice for simulation - if (_flag_enable_mc_motors) { - set_max_mc(1940); - _flag_enable_mc_motors = false; - }*/ - -} - -void Kitepower::update_fw_state() -{ - VtolType::update_fw_state(); - _mc_roll_weight = 0.0f; - _mc_pitch_weight = 0.0f; - _mc_yaw_weight = 0.0f; - _mc_throttle_weight = 0.0f; - - /*for the moment let the motor spin during fixed wing, nice for simulation - if (!_flag_enable_mc_motors) { - set_max_mc(940); - _flag_enable_mc_motors = true; - }*/ - - -} - -/** -* Write data to actuator output topic. -*/ - -void Kitepower::fill_actuator_outputs() -{ - //Kitepower: fail saife mode needs to be added -> probably later on dronecore side - //fill multicopter controls, index 0 - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]*_mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]*_mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]*_mc_yaw_weight; //needs to be tested - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]*_mc_throttle_weight; - - //fill in fixed wing controls, important actuator for - //in the fw_att_controller there is a coordinate frame transformation while switching to fw mode (neutral position is switched 90° around pitch axis) - //Thus we have a coordinate frame for mc and one - //for fw. Thus mc_yaw!=fw_yaw. - _actuators_out_1->timestamp = _actuators_fw_in->timestamp; - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =0.0f; - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]*(1-_mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]*(1-_mc_yaw_weight); //yaw axis in multicopter mode means roll axis in fixedwing, thus no roll control - -} - - -/** -* Disable all multirotor motors when in fw mode. -*/ -/*for the moment let the motor spin during fixed wing, nice for simulation -void -Kitepower::set_max_mc(unsigned pwm_value) -{ - int ret; - unsigned servo_count; - const char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_WARN("can't open %s", dev); - } - - ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count = _params->vtol_motor_count; - } - - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) { - PX4_WARN("failed setting max values"); - } - - px4_close(fd); -}*/ diff --git a/src/modules/vtol_att_control/kitepower.h b/src/modules/vtol_att_control/kitepower.h deleted file mode 100644 index d3729e1f2e0d..000000000000 --- a/src/modules/vtol_att_control/kitepower.h +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** -* @file kitepower.h -* -* @author Roman Bapst -* @author Gabriel König -* -*/ - -#ifndef KITEPOWER_H -#define KITEPOWER_H - -#include "vtol_type.h" -#include /** is it necsacery? **/ -#include -#include - -class Kitepower : public VtolType -{ - -public: - Kitepower(VtolAttitudeControl *_att_controller); - ~Kitepower(); - - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void update_mc_state(); - virtual void update_fw_state(); - virtual void fill_actuator_outputs(); - virtual void waiting_on_tecs(); - -private: - - struct { - float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ - float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ - - } _params_kitepower; - - struct { - param_t airspeed_trans; - param_t airspeed_blend_start; - - } _params_handles_kitepower; - - enum vtol_mode { - MC_MODE = 0, /**< vtol is in multicopter mode */ - TRANSITION_FRONT, /**< vtol is in front transition part 1 mode */ - TRANSITION_BACK, /**< vtol is in back transition mode */ - FW_MODE /**< vtol is in fixed wing mode */ - }; - - struct { - vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ - hrt_abstime transition_start; /**< absoulte time at which front transition started */ - } _vtol_schedule; - - float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ - - bool _flag_enable_mc_motors; - - - float _thrust_transition_start; // throttle value when we start the front transition - float _yaw_transition; // yaw angle in which transition will take place - float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) - - - /** should this anouncement stay? **/ - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /** - * Speed estimation for propwash controlled surfaces. - */ - void calc_tot_airspeed(); - - - /** is this one still needed? */ - void scale_mc_output(); - - /** - * Update parameters. - */ - virtual void parameters_update(); - - void set_max_mc(unsigned pwm_value); - - /** - * Update external VTOL state - */ - void update_external_VTOL_state(); - -}; -#endif diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 6a28ef27fa85..3588905900ca 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -86,8 +86,7 @@ VtolAttitudeControl::VtolAttitudeControl() if (_params.vtol_type == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); - } else if (_params.vtol_type ==vtol_type::KITEPOWER) { - _vtol_type = new Kitepower(this); + } else if (_params.vtol_type == vtol_type::TILTROTOR) { _vtol_type = new Tiltrotor(this); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index fd0df8f6c93e..492259356aba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -81,7 +81,6 @@ #include "tiltrotor.h" #include "tailsitter.h" #include "standard.h" -#include "kitepower.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 8998cbfe6f9e..65ec471026c0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -87,103 +87,13 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f); /** -<<<<<<< HEAD * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) -||||||| merged common ancestors - * Motor max power - * - * Indicates the maximum power the motor is able to produce. Used to calculate - * propeller efficiency map. - * - * @unit W - * @min 1 - * @max 10000 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); - -/** - * Propeller efficiency parameter - * - * Influences propeller efficiency at different power settings. Should be tuned beforehand. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); - -/** - * Total airspeed estimate low-pass filter gain - * - * Gain for tuning the low-pass filter for the total airspeed estimate - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); - -/** - * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) -======= - * Motor max power - * - * Indicates the maximum power the motor is able to produce. Used to calculate - * propeller efficiency map. - * - * @unit W - * @min 1 - * @max 10000 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); - -/** - * Propeller efficiency parameter - * - * Influences propeller efficiency at different power settings. Should be tuned beforehand. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); - -/** - * Total airspeed estimate low-pass filter gain - * - * Gain for tuning the low-pass filter for the total airspeed estimate - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); - -/** - * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2, Kitepower=3) ->>>>>>> gamma_rigid_wing * * @value 0 Tailsitter * @value 1 Tiltrotor * @value 2 Standard - * @value 3 Kitepower - * @min 0 - * @max 3 + * @max 2 * @decimal 0 * @group VTOL Attitude Control */ diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1fa88cc9d74d..1aa7d18e8ff5 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -73,9 +73,8 @@ enum mode { enum vtol_type { TAILSITTER = 0, - TILTROTOR, - STANDARD, - KITEPOWER + TILTROTOR, + STANDARD }; class VtolAttitudeControl;