diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index a5d412fe21..83c1625b0f 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -43,12 +43,6 @@ pipeline { archive: true ] - def nuttx_builds_other = [ - target: ["px4_esc-v1_default"], - image: docker_images.nuttx, - archive: false - ] - def snapdragon_builds = [ target: ["atlflight_eagle_qurt-default", "atlflight_eagle_default"], image: docker_images.snapdragon, @@ -56,7 +50,7 @@ pipeline { ] def docker_builds = [ - armhf_builds, base_builds, nuttx_builds_archive, nuttx_builds_other, snapdragon_builds + armhf_builds, base_builds, nuttx_builds_archive, snapdragon_builds ] for (def build_type = 0; build_type < docker_builds.size(); build_type++) { diff --git a/Makefile b/Makefile index 2f6951fb16..779f7e7d9d 100644 --- a/Makefile +++ b/Makefile @@ -263,7 +263,6 @@ misc_qgc_extra_firmware: \ # Other NuttX firmware alt_firmware: \ check_px4_cannode-v1_default \ - check_px4_esc-v1_default \ sizes # builds with RTPS diff --git a/boards/px4/esc-v1/default.cmake b/boards/px4/esc-v1/default.cmake deleted file mode 100644 index dcdb9a8c77..0000000000 --- a/boards/px4/esc-v1/default.cmake +++ /dev/null @@ -1,60 +0,0 @@ - -add_definitions( - -DFLASH_BASED_PARAMS - -DPARAM_NO_ORB - -DPARAM_NO_AUTOSAVE - -DPARAMETER_BUFFER_SIZE=1024 -) - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} - ) - -# Bring in common uavcan hardware identity definitions -include(px4_git) -px4_add_git_submodule(TARGET git_uavcan_board_ident PATH "cmake/configs/uavcan_board_ident") -include(configs/uavcan_board_ident/px4esc-v1) - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) - -include(px4_make_uavcan_bootloader) -px4_make_uavcan_bootloadable( - BOARD px4_esc-v1 - BIN ${PX4_BINARY_DIR}/px4_esc-v1.bin - HWNAME ${uavcanblid_name} - HW_MAJOR ${uavcanblid_hw_version_major} - HW_MINOR ${uavcanblid_hw_version_minor} - SW_MAJOR ${uavcanblid_sw_version_major} - SW_MINOR ${uavcanblid_sw_version_minor} -) - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL esc-v1 - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - - DRIVERS - bootloaders - uavcanesc - - MODULES - - SYSTEMCMDS - config - reboot - param - top - ver - work_queue - - ) diff --git a/boards/px4/esc-v1/firmware.prototype b/boards/px4/esc-v1/firmware.prototype deleted file mode 100644 index aee968cdc9..0000000000 --- a/boards/px4/esc-v1/firmware.prototype +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_id": 25, - "magic": "ESCv1", - "description": "Firmware for the PX4ESCV1 board", - "image": "", - "build_time": 0, - "summary": "PX4ESCv1", - "version": "0.1", - "image_size": 0, - "image_maxsize": 475136, - "git_identity": "", - "board_revision": 0 -} diff --git a/boards/px4/esc-v1/nuttx-config/Kconfig b/boards/px4/esc-v1/nuttx-config/Kconfig deleted file mode 100644 index 4b7aa9e2d1..0000000000 --- a/boards/px4/esc-v1/nuttx-config/Kconfig +++ /dev/null @@ -1,22 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if CONFIG_ARCH_BOARD_PX4_ESC_V1 - -config BOARD_HAS_PROBES - bool "Board provides GPIO or other Hardware for signaling to timing analyze." - default y - ---help--- - This board provides GPIO TEST1(PD2), TEST2(PB3), TEST3(PB4), TEST4(PC12) as PROBE_1-4 to provide timing signals from selected drivers. - -config BOARD_USE_PROBES - bool "Enable the use provides GPIO TEST1(PD2), TEST2(PB3), TEST3(PB4), TEST4(PC12) as PROBE_1-4 to provide timing signals from selected drivers" - default n - depends on BOARD_HAS_PROBES - - ---help--- - Select to use GPIO TEST1(PD2), TEST2(PB3), TEST3(PB4), TEST4(PC12) as PROBE_1-4 to provide timing signals from selected drivers. - -endif diff --git a/boards/px4/esc-v1/nuttx-config/include/README.txt b/boards/px4/esc-v1/nuttx-config/include/README.txt deleted file mode 100644 index 9e38b31606..0000000000 --- a/boards/px4/esc-v1/nuttx-config/include/README.txt +++ /dev/null @@ -1,2 +0,0 @@ -This directory contains header files unique to the -PX4 Esc V1.6 board using STM32F446RET6 diff --git a/boards/px4/esc-v1/nuttx-config/include/board.h b/boards/px4/esc-v1/nuttx-config/include/board.h deleted file mode 100644 index 88b638c1b8..0000000000 --- a/boards/px4/esc-v1/nuttx-config/include/board.h +++ /dev/null @@ -1,358 +0,0 @@ -/************************************************************************************ - * nuttx-configs/px4esc-v1/include/board.h - * - * Copyright (C) 2015 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * David Sidrane - * - * 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 NuttX 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. - * - ************************************************************************************/ - -#ifndef __CONFIGS_PX4ESC_V1_INCLUDE_BOARD_H -#define __CONFIGS_PX4ESC_V1_INCLUDE_BOARD_H - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#ifndef __ASSEMBLY__ -# include -#endif -#include "stm32_rcc.h" -#include "stm32_sdio.h" -#include "stm32.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/* Clocking *************************************************************************/ -/* The PX4ESC uses a 8MHz crystal connected to the HSE. - * - * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: - * System Clock source : PLL (HSE) - * SYSCLK(Hz) : 180000000 Determined by PLL configuration - * HCLK(Hz) : 180000000 (STM32_RCC_CFGR_HPRE) - * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) - * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) - * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) - * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) - * PLLM : 4 (STM32_PLLCFG_PLLM) - * PLLN : 180 (STM32_PLLCFG_PLLN) - * PLLP : 2 (STM32_PLLCFG_PLLP) - * PLLQ : 2 (STM32_PLLCFG_PPQ) - * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK - * Flash Latency(WS) : 5 - * Prefetch Buffer : OFF - * Instruction cache : ON - * Data cache : ON - * Require 48MHz for USB OTG FS, : Use PLLSA1M - */ - -//TODO(Need to define and add the PLLSAIM ); - -/* HSI - 16 MHz RC factory-trimmed - * LSI - 32 KHz RC - * HSE - On-board crystal frequency is 24MHz - * LSE - not installed - */ - -#define STM32_BOARD_XTAL 8000000ul - -#define STM32_HSI_FREQUENCY 16000000ul -#define STM32_LSI_FREQUENCY 32000 -#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL - -/* Main PLL Configuration. - * - * PLL source is HSE - * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN - * = (8,000,000 / 4) * 180 - * = 360,000,000 - * SYSCLK = PLL_VCO / PLLP - * = 360,000,000/ 2 = 180,000,000 - * USB OTG FS will use PLLSA1M - * - */ -// - -#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(4) -#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(180) -#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 -#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) -#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) - -/* Configure factors for PLLSAI clock */ - -#define STM32_RCC_PLLSAICFGR_PLLSAIM RCC_PLLSAICFGR_PLLSAIM(4) -#define STM32_RCC_PLLSAICFGR_PLLSAIN RCC_PLLSAICFGR_PLLSAIN(96) -#define STM32_RCC_PLLSAICFGR_PLLSAIP RCC_PLLSAICFGR_PLLSAIP(4) -#define STM32_RCC_PLLSAICFGR_PLLSAIQ RCC_PLLSAICFGR_PLLSAIQ(2) - -/* Configure Dedicated Clock Configuration Register */ - -#define STM32_RCC_DCKCFGR_PLLI2SDIVQ RCC_DCKCFGR_PLLI2SDIVQ(1) -#define STM32_RCC_DCKCFGR_PLLSAIDIVQ RCC_DCKCFGR_PLLSAIDIVQ(1) -#define STM32_RCC_DCKCFGR_SAI1SRC RCC_DCKCFGR_SAI1SRC_PLLSAI -#define STM32_RCC_DCKCFGR_SAI2SRC RCC_DCKCFGR_SAI2SRC_PLLSAI -#define STM32_RCC_DCKCFGR_TIMPRE 0 -#define STM32_RCC_DCKCFGR_I2S1SRC RCC_DCKCFGR_SAI1SRC_PLL -#define STM32_RCC_DCKCFGR_I2S2SRC RCC_DCKCFGR_SAI2SRC_PLL - - - -/* Configure factors for PLLI2S clock */ - - -#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) -#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) -#define STM32_RCC_PLLI2SCFGR_PLLI2SP RCC_PLLI2SCFGR_PLLI2SP(2) -#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) -#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) - -/* Configure Dedicated Clock Configuration Register 2 */ - -#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB -#define STM32_RCC_DCKCFGR2_CECSEL RCC_DCKCFGR2_CECSEL_HSI -#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLLSAI -#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ -#define STM32_RCC_DCKCFGR2_SPDIFRXSEL RCC_DCKCFGR2_SPDIFRXSEL_PLL - -#define STM32_SYSCLK_FREQUENCY 180000000ul - -/* AHB clock (HCLK) is SYSCLK (180MHz) */ - -#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ -#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY -#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ - -/* APB1 clock (PCLK1) is HCLK/4 (45MHz) */ - -#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ -#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) - -/* Timers driven from APB1 will be twice PCLK1 */ - -#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) - -/* APB2 clock (PCLK2) is HCLK/2 (90MHz) */ - -#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ -#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) - -/* Timers driven from APB2 will be twice PCLK2 */ - -#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) - -/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. - * Note: TIM1,8-11 are on APB2, others on APB1 - */ - -#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN -#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN -#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN -#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN -#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN -#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN -#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN -#define BOARD_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN -#define BOARD_TIM9_FREQUENCY STM32_APB2_TIM9_CLKIN -#define BOARD_TIM10_FREQUENCY STM32_APB2_TIM10_CLKIN -#define BOARD_TIM11_FREQUENCY STM32_APB2_TIM11_CLKIN -#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN -#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN -#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN - -/* Leds *************************************************************************/ - -/* LED index values for use with board_setled() */ - -#define BOARD_LED1 0 -#define BOARD_LED_RED BOARD_LED1 -#define BOARD_LED2 1 -#define BOARD_LED_GREEN BOARD_LED2 -#define BOARD_LED3 2 -#define BOARD_LED_BLUE BOARD_LED3 -#define BOARD_NLEDS 3 - -/* LED bits for use with board_setleds() */ - -#define BOARD_LED_RED_BIT (1 << BOARD_LED_RED) -#define BOARD_LED_GREEN_BIT (1 << BOARD_LED_GREEN) -#define BOARD_LED_BLUE_BIT (1 << BOARD_LED_BLUE) - -/* TODO:define these - * These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is - * - * defined. In that case, the usage by the board port is as follows: - * - * SYMBOL Meaning LED state - * Red Green Blue - * ------------------------ -------------------------- ------ ------ ----*/ - -#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ -#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ -#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ -#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ -#define LED_INIRQ 4 /* In an interrupt N/C GLOW N/C */ -#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ -#define LED_ASSERTION 6 /* An assertion failed GLOW GLOW N/C */ -#define LED_PANIC 7 /* The system has crashed Blk OFF N/C */ -#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ - -/* - * Thus if the blue is statically on, NuttX has successfully booted and is, - * apparently, running normally. If the Red LED is flashing at - * approximately 2Hz, then a fatal error has been detected and the system - * has halted. - * - */ - - -/* Alternate function pin selections ************************************************/ - -/* UARTs */ - -#define GPIO_USART3_RX GPIO_USART3_RX_4 -#define GPIO_USART3_TX GPIO_USART3_TX_1 - -/* CAN - * - * CAN1 is routed to the onboard transceiver. - * CAN2 is routed to the onboard transceiver. - */ - -#define GPIO_CAN1_RX GPIO_CAN1_RX_2 -#define GPIO_CAN1_TX GPIO_CAN1_TX_2 -#define GPIO_CAN2_RX GPIO_CAN2_RX_2 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 - -/* TIMERS */ - -#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 -#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 -#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 - - -#if defined(CONFIG_BOARD_USE_PROBES) -# define PROBE_N(n) (1<<((n)-1)) -# define PROBE_1 GPIO_TEST1 -# define PROBE_2 GPIO_TEST2 -# define PROBE_3 GPIO_TEST3 -# define PROBE_3 GPIO_TEST4 - -# define PROBE_INIT(mask) \ - do { \ - if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ - if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ - if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ - } while(0) - -# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) -# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) -#else -# define PROBE_INIT(mask) -# define PROBE(n,s) -# define PROBE_MARK(n) -#endif - -/************************************************************************************ - * Public Data - ************************************************************************************/ - -#ifndef __ASSEMBLY__ - -#undef EXTERN -#if defined(__cplusplus) -#define EXTERN extern "C" -extern "C" -{ -#else -#define EXTERN extern -#endif - -/************************************************************************************ - * Public Function Prototypes - ************************************************************************************/ -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the initialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -void stm32_boardinitialize(void); - -/************************************************************************************ - * Name: stm32_ledinit, stm32_setled, and stm32_setleds - * - * Description: - * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If - * CONFIG_ARCH_LEDS is not defined, then the following interfaces are available to - * control the LEDs from user applications. - * - ************************************************************************************/ - -#ifndef CONFIG_ARCH_LEDS -void stm32_led_initialize(void); -void stm32_setled(int led, bool ledon); -void stm32_setleds(uint8_t ledset); -#endif - -#if !defined(CONFIG_NSH_LIBRARY) -int app_archinitialize(void); -#else -#define app_archinitialize() (-ENOSYS) -#endif - -#undef EXTERN -#if defined(__cplusplus) -} -#endif - -#endif /* __ASSEMBLY__ */ -#endif /* __CONFIGS_PX4ESC_V1_INCLUDE_BOARD_H */ diff --git a/boards/px4/esc-v1/nuttx-config/nsh/defconfig b/boards/px4/esc-v1/nuttx-config/nsh/defconfig deleted file mode 100644 index 796d221b21..0000000000 --- a/boards/px4/esc-v1/nuttx-config/nsh/defconfig +++ /dev/null @@ -1,160 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_OS_API is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_CMP is not set -# CONFIG_NSH_DISABLE_DD is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_HEXDUMP is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_LOSETUP is not set -# CONFIG_NSH_DISABLE_MKFIFO is not set -# CONFIG_NSH_DISABLE_MKRD is not set -# CONFIG_NSH_DISABLE_PS is not set -# CONFIG_NSH_DISABLE_PUT is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_UNAME is not set -# CONFIG_NSH_DISABLE_XD is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32=y -CONFIG_ARCH_CHIP_STM32F446R=y -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CUSTOM_LEDS=y -CONFIG_BOARD_LOOPSPERMSEC=5483 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_BUILTIN_PROXY_STACKSIZE=4096 -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x60c7 -CONFIG_CDCACM_PRODUCTSTR="PX4 ESC v1.6" -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 -CONFIG_CDCACM_VENDORID=0x1d50 -CONFIG_CDCACM_VENDORSTR="Zubax" -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_FDCLONE_STDIO=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=16 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_NAME_MAX=93 -CONFIG_NFILE_DESCRIPTORS=20 -CONFIG_NFILE_STREAMS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_NXFONTS_DISABLE_16BPP=y -CONFIG_NXFONTS_DISABLE_1BPP=y -CONFIG_NXFONTS_DISABLE_24BPP=y -CONFIG_NXFONTS_DISABLE_2BPP=y -CONFIG_NXFONTS_DISABLE_32BPP=y -CONFIG_NXFONTS_DISABLE_4BPP=y -CONFIG_NXFONTS_DISABLE_8BPP=y -CONFIG_PIPES=y -CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=4096 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_PREALLOC_TIMERS=20 -CONFIG_PREALLOC_WDOGS=20 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_DEFAULT=4096 -CONFIG_PTHREAD_STACK_MIN=4096 -CONFIG_RAM_SIZE=131072 -CONFIG_RAM_START=0x20000000 -CONFIG_RAW_BINARY=y -CONFIG_READLINE_CMD_HISTORY=y -CONFIG_READLINE_TABCOMPLETION=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=192 -CONFIG_SCHED_HPWORKSTACKSIZE=4096 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=4096 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_FORCEPOWER=y -CONFIG_STM32_I2SPLL=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=y -CONFIG_STM32_OTGFS=y -CONFIG_STM32_PWR=y -CONFIG_STM32_RTC=y -CONFIG_STM32_RTC_HSECLOCK=y -CONFIG_STM32_RTC_MAGIC_REG=1 -CONFIG_STM32_SAIPLL=y -CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_TIM1=y -CONFIG_STM32_TIM3=y -CONFIG_STM32_USART3=y -CONFIG_SYMTAB_ORDEREDBYNAME=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=4096 -CONFIG_TIME_EXTENDED=y -CONFIG_USART3_RXBUFSIZE=32 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_SERIAL_CONSOLE=y -CONFIG_USART3_TXBUFSIZE=32 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=4096 -CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_WATCHDOG=y diff --git a/boards/px4/esc-v1/nuttx-config/scripts/script.ld b/boards/px4/esc-v1/nuttx-config/scripts/script.ld deleted file mode 100644 index 9382c28d9c..0000000000 --- a/boards/px4/esc-v1/nuttx-config/scripts/script.ld +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** - * nuttx-configs/px4esc-v1/scripts/ld.script - * - * Copyright (C) 2015 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/* The STM32F446RE has 512KiB of FLASH beginning at address 0x0800:0000 and - * 112KiB of SRAM beginning at address 0x2000:0000. With an addtional 16 KiB - * located at 0x2001:c000. - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point - * in the 0x0800:0000 address range. - * The bootloader only uses the first 16KiB of flash. - * Paramater storage will use the next 2 16KiB Sectors. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x0800C000, LENGTH = 464K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K - sram1 (rwx) : ORIGIN = 0x2001C000, LENGTH = 16K -} - -OUTPUT_ARCH(arm) - -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - . = ALIGN(8); - /* - * This section positions the app_descriptor_t used - * by the make_can_boot_descriptor.py tool to set - * the application image's descriptor so that the - * uavcan bootloader has the ability to validate the - * image crc, size etc - */ - KEEP(*(.app_descriptor)) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - /* - * This is a hack to make the newlib libm __errno() call - * use the NuttX get_errno_ptr() function. - */ - __errno = get_errno_ptr; - } > flash - -/* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - /* The STM32F446RE has 112KiB of SRAM beginning at the following address */ - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - . = ALIGN(4); - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/boards/px4/esc-v1/src/CMakeLists.txt b/boards/px4/esc-v1/src/CMakeLists.txt deleted file mode 100644 index c8b0bf013e..0000000000 --- a/boards/px4/esc-v1/src/CMakeLists.txt +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -add_library(drivers_board - init.c - led.c - usb.c -) - -target_link_libraries(drivers_board - PRIVATE - drivers__led # drv_led_start - px4_layer - ) diff --git a/boards/px4/esc-v1/src/board_config.h b/boards/px4/esc-v1/src/board_config.h deleted file mode 100644 index 9b5c85f214..0000000000 --- a/boards/px4/esc-v1/src/board_config.h +++ /dev/null @@ -1,350 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 board_config.h - * - * PX4ESCv1 internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -#if STM32_NSPI < 1 -# undef CONFIG_STM32_SPI1 -# undef CONFIG_STM32_SPI2 -#elif STM32_NSPI < 2 -# undef CONFIG_STM32_SPI2 -#endif - -/* High-resolution timer - */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ - -/* GPIO *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PA[03] PA3/TIM2_CH4/TIM5_CH4/TIM9_CH2/USART2_RX 17 RC_PWM - * PA[04] PA4/SPI1_NSS 20 OC_ADJ - * PA[05] PA5/TIM2_CH1/TIM2_ETR/TIM8_CH1 21 EN_GATE - * PA[06] PA6/TIM1_BKIN/TIM3_CH1/TIM8_BKIN/SPI1_MISO 22 DC_CAL - * - */ - -#define GPIO_RC_PWM (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTA | GPIO_PIN3) -#define GPIO_OC_ADJ (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTA | GPIO_PIN4 | GPIO_OUTPUT_CLEAR) -#define GPIO_EN_GATE (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTA | GPIO_PIN5 | GPIO_OUTPUT_CLEAR) -#define GPIO_DC_CAL (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTA | GPIO_PIN6 | GPIO_OUTPUT_CLEAR) - - -/* GPIO *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PB[02] PB2/TIM2_CH4/SPI3_MOSI 28 GAIN - * PB[03] PB3/TIM2_CH2/I2C2_SDA/SPI1_SCK 55 TEST2 - * PB[04] PB4/TIM3_CH1/I2C3_SDA/SPI1_MISO/SPI3_MISO 56 TEST3 - * - */ - -#define GPIO_GAIN (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTB | GPIO_PIN2 | GPIO_OUTPUT_CLEAR) -#define GPIO_TEST2 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTB | GPIO_PIN3 | GPIO_OUTPUT_CLEAR) -#define GPIO_TEST3 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTB | GPIO_PIN4 | GPIO_OUTPUT_CLEAR) - -/* CAN ************************************************************************ * - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ------------------------ - * - * PB[05] PB5/TIM3_CH2/SPI1_MOSI/CAN2_RX 57 CAN2_RX - * PB[06] PB6/TIM4_CH1/USART1_TX/CAN2_TX 58 CAN2_TX - * PB[07] PB7/TIM2_CH2/TIM4_CH4/TIM11_CH1/I2C1_SDA 59 WAIT_GETNODEINFO - * PB[08] PB8/TIM4_CH3/I2C1_SCL/CAN1_RX 61 CAN1_RX - * PB[09] PB9/TIM4_CH4/I2C1_SDA/CAN1_TX 62 CAN1_TX - * - */ - -#define GPIO_WAIT_GETNODEINFO (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTB | GPIO_PIN7) - -/* UART3 ************************************************************************ * - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ------------------------ - * - * PB[10] PB10/TIM2_CH3/I2C2_SCL/SPI2_SCK/I2S2_CK/USART3_TX 29 DBG_TX - * PC[05] PC5/USART3_RX/SPDIFRX_IN3/FMC_SDCKE0 25 DBG_RX - * - */ - - -/* Analog *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PC[00] PC0/ADC123_IN10 8 TEMP_SENS - * PC[01] PC1/ADC123_IN11/SPI3_MOSI/SPI2_MOSI 9 VBAT_SENS - * PC[02] PC2/ADC123_IN12/SPI2_MISO, 10 CURR_SENS2 - * PC[03] PC3/ADC123_IN13/SPI2_MOSI 11 CURR_SENS1 - * - */ - -/* GPIO *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PC[06] TIM3_CH1/TIM8_CH1/USART6_TX 37 RPM - * - */ - -#define GPIO_RPM (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN6) - -/* LEDs *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PC[07] PC7/TIM3_CH2/TIM8_CH2 38 LED_RED - * PC[08] PC8/TIM3_CH3/TIM8_CH3 39 LED_GREEN - * PC[09] PC9/TIM3_CH4/TIM8_CH4 40 LED_BLUE - */ - -#define GPIO_LED1 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTC | GPIO_PIN7 | GPIO_OUTPUT_CLEAR) -#define GPIO_LED_RED GPIO_LED1 - -#define GPIO_LED2 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTC | GPIO_PIN8 | GPIO_OUTPUT_CLEAR) -#define GPIO_LED_GREEN GPIO_LED2 - -#define GPIO_LED3 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTC | GPIO_PIN9 | GPIO_OUTPUT_CLEAR) -#define GPIO_LED_BLUE GPIO_LED3 - - -/* GPIO *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PC[10] PC10/SPI3_SCK/USART3_TX/UART4_TX/ 51 HWID0 - * PC[11] PC11/SPI3_MISO/USART3_RX/UART4_RX 52 HWID1 - * PC[12] PC12/I2C2_SDA/SPI3_MOSI/USART3_CK/UART5_TX 53 TEST4 - * PC[13] PC13/TAMP_1/WKUP1 2 PWRGD - * PC[14] PC14/OSC32_IN 3 OCTW - * PC[15] PC15/OSC32_OUT 4 FAULT - * - */ - -#define GPIO_HWID0 (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN10) -#define GPIO_HWID1 (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN11) -#define GPIO_TEST4 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTC | GPIO_PIN12 | GPIO_OUTPUT_CLEAR) - -#define GPIO_PWRGD (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN13) -#define GPIO_OCTW (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN14) -#define GPIO_FAULT (GPIO_INPUT | GPIO_PULLUP | GPIO_PORTC | GPIO_PIN15) - -/* GPIO *********************************************************************** - * - * GPIO Function MPU Board - * Pin # Name - * -- ----- -------------------------------- ---------------------- - * - * PD[02] PD2/TIM3_ETR/UART5_RX/SDIO_CMD 54 TEST1 - */ -#define GPIO_TEST1 (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_50MHz | GPIO_PORTD | GPIO_PIN2 | GPIO_OUTPUT_CLEAR) - -__BEGIN_DECLS - -/************************************************************************************ - * Public Types - ************************************************************************************/ - -/************************************************************************************ - * Public data - ************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins. - * - ************************************************************************************/ - -#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \ - defined(CONFIG_STM32_SPI3) -void weak_function board_spiinitialize(void); -#endif - -/************************************************************************************ - * Name: stm32_usbinitialize - * - * Description: - * Called to setup USB-related GPIO pins. - * - ************************************************************************************/ - -void stm32_usbinitialize(void); - -/************************************************************************************ - * Name: stm32_usb_set_pwr_callback() - * - * Description: - * Called to setup set a call back for USB power state changes. - * - * Inputs: - * pwr_changed_handler: An interrupt handler that will be called on VBUS power - * state changes. - * - ************************************************************************************/ - -void stm32_usb_set_pwr_callback(xcpt_t pwr_changed_handler); - -/**************************************************************************** - * Name: stm32_led_initialize - * - * Description: - * This functions is called very early in initialization to perform board- - * specific initialization of LED-related resources. This includes such - * things as, for example, configure GPIO pins to drive the LEDs and also - * putting the LEDs in their correct initial state. - * - * NOTE: In most architectures, LED initialization() is called from - * board-specific initialization and should, therefore, have the name - * _led_intialize(). But there are a few architectures where the - * LED initialization function is still called from common chip - * architecture logic. This interface is not, however, a common board - * interface in any event and the name board_autoled_initialization is - * deprecated. - * - * Input Parameters: - * None - * - * Returned Value: - * None - * - ****************************************************************************/ - -#ifdef CONFIG_ARCH_LEDS -void board_autoled_initialize(void); -#endif - -/************************************************************************************ - * Name: stm32_can_initialize - * - * Description: - * Called at application startup time to initialize the CAN functionality. - * - ************************************************************************************/ - -#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2)) -int board_can_initialize(void); -#endif - -/************************************************************************************ - * Name: board_button_initialize - * - * Description: - * Called at application startup time to initialize the Buttons functionality. - * - ************************************************************************************/ - -#if defined(CONFIG_ARCH_BUTTONS) -void board_button_initialize(void); -#endif - -/**************************************************************************** - * Name: usbmsc_archinitialize - * - * Description: - * Called from the application system/usbmc or the boards_nsh if the - * application is not included. - * Perform architecture specific initialization. This function must - * configure the block device to export via USB. This function must be - * provided by architecture-specific logic in order to use this add-on. - * - ****************************************************************************/ - -#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_USBMSC) -int usbmsc_archinitialize(void); -#endif - -/**************************************************************************** - * Name: composite_archinitialize - * - * Description: - * Called from the application system/composite or the boards_nsh if the - * application is not included. - * Perform architecture specific initialization. This function must - * configure the block device to export via USB. This function must be - * provided by architecture-specific logic in order to use this add-on. - * - ****************************************************************************/ - -#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_COMPOSITE) -extern int composite_archinitialize(void); -#endif - -#include - -#endif /* __ASSEMBLY__ */ - -__END_DECLS diff --git a/boards/px4/esc-v1/src/init.c b/boards/px4/esc-v1/src/init.c deleted file mode 100644 index ba93448724..0000000000 --- a/boards/px4/esc-v1/src/init.c +++ /dev/null @@ -1,187 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 px4flow_init.c - * - * PX4ESC-specific early startup code. This file implements the - * board_app_initialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialization. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include "board_config.h" -#include "stm32_uart.h" - -#include - -#include -#include - -#include - -#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) -#endif - -# if defined(FLASH_BASED_PARAMS) -# include -#endif - -#include "board_config.h" - -/* todo: This is constant but not proper */ -__BEGIN_DECLS -extern void led_off(int led); -__END_DECLS - - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * 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 */ - board_autoled_initialize(); -#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \ - defined(CONFIG_STM32_SPI3) - board_spiinitialize(); -#endif -} - - -__EXPORT void board_initialize(void) -{ -} - -/**************************************************************************** - * Name: board_app_initialize - * - * Description: - * Perform application specific initialization. This function is never - * called directly from application code, but only indirectly via the - * (non-standard) boardctl() interface using the command BOARDIOC_INIT. - * - * Input Parameters: - * arg - The boardctl() argument is passed to the board_app_initialize() - * implementation without modification. The argument has no - * meaning to NuttX; the meaning of the argument is a contract - * between the board-specific initalization logic and the the - * matching application logic. The value cold be such things as a - * mode enumeration value, a set of DIP switch switch settings, a - * pointer to configuration data read from a file or serial FLASH, - * or whatever you would like to do with it. Every implementation - * should accept zero/NULL as a default configuration. - * - * Returned Value: - * Zero (OK) is returned on success; a negated errno value is returned on - * any failure to indicate the nature of the failure. - * - ****************************************************************************/ - -__EXPORT int board_app_initialize(uintptr_t arg) -{ - - int result = OK; - - px4_platform_init(); - - /* set up the serial DMA polling */ - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - -#if defined(FLASH_BASED_PARAMS) - static sector_descriptor_t sector_map[] = { - {1, 16 * 1024, 0x08004000}, - {2, 16 * 1024, 0x08008000}, - {0, 0, 0}, - }; - static uint8_t param_buffer[PARAMETER_BUFFER_SIZE]; - - parameter_flashfs_init(sector_map, param_buffer, sizeof(param_buffer)); -#endif - return result; -} diff --git a/boards/px4/esc-v1/src/led.c b/boards/px4/esc-v1/src/led.c deleted file mode 100644 index f99f04672d..0000000000 --- a/boards/px4/esc-v1/src/led.c +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 px4esc_led.c - * - * PX4ESC LED backend. - */ - -#include - -#include -#include - -#include "stm32.h" -#include "board_config.h" - -#include - -#include - -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); -__END_DECLS - -static uint32_t g_ledmap[] = { - GPIO_LED_RED, // Indexed by BOARD_LED_RED - GPIO_LED_GREEN, // Indexed by BOARD_LED_GREEN - GPIO_LED_BLUE, // Indexed by BOARD_LED_BLUE -}; - -__EXPORT void led_init(void) -{ - /* Configure LED GPIOs for output */ - for (size_t l = 0; l < arraySize(g_ledmap); l++) { - stm32_configgpio(g_ledmap[l]); - } -} - -__EXPORT void board_autoled_initialize(void) -{ - led_init(); -} - -static void phy_set_led(int led, bool state) -{ - /* Pull Down to switch on */ - stm32_gpiowrite(g_ledmap[led], !state); -} - -static bool phy_get_led(int led) -{ - - return !stm32_gpioread(g_ledmap[led]); -} - -__EXPORT void led_on(int led) -{ - phy_set_led(led, true); -} - -__EXPORT void led_off(int led) -{ - phy_set_led(led, false); -} - -__EXPORT void led_toggle(int led) -{ - - phy_set_led(led, !phy_get_led(led)); -} - -static bool g_initialized; - -// Nuttx Usages - -__EXPORT -void board_autoled_on(int led) -{ - switch (led) { - default: - case LED_STARTED: - case LED_HEAPALLOCATE: - phy_set_led(BOARD_LED_BLUE, true); - break; - - case LED_IRQSENABLED: - phy_set_led(BOARD_LED_GREEN, true); - break; - - case LED_STACKCREATED: - phy_set_led(BOARD_LED_GREEN, true); - phy_set_led(BOARD_LED_BLUE, true); - g_initialized = true; - break; - - case LED_INIRQ: - case LED_SIGNAL: - phy_set_led(BOARD_LED_GREEN, true); - break; - - case LED_ASSERTION: - phy_set_led(BOARD_LED_RED, true); - phy_set_led(BOARD_LED_GREEN, true); - break; - - case LED_PANIC: - phy_set_led(BOARD_LED_RED, true); - break; - - case LED_IDLE : /* IDLE */ - phy_set_led(BOARD_LED_RED, true); - phy_set_led(BOARD_LED_BLUE, true); - break; - } -} - -/**************************************************************************** - * Name: board_autoled_off - ****************************************************************************/ - - -__EXPORT void board_autoled_off(int led) -{ - switch (led) { - default: - case LED_STARTED: - case LED_HEAPALLOCATE: - case LED_IRQSENABLED: - phy_set_led(BOARD_LED_BLUE, false); - - case LED_STACKCREATED: - break; - - case LED_INIRQ: - case LED_SIGNAL: - case LED_ASSERTION: - phy_set_led(BOARD_LED_RED, false); - phy_set_led(BOARD_LED_GREEN, false); - - break; - - case LED_PANIC: - phy_set_led(BOARD_LED_RED, false); - phy_set_led(BOARD_LED_GREEN, false); - break; - - case LED_IDLE: /* IDLE */ - phy_set_led(BOARD_LED_GREEN, g_initialized); - break; - } -} diff --git a/boards/px4/esc-v1/src/usb.c b/boards/px4/esc-v1/src/usb.c deleted file mode 100644 index 68af9d758e..0000000000 --- a/boards/px4/esc-v1/src/usb.c +++ /dev/null @@ -1,107 +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 px4esc_usb.c - * - * Board-specific USB functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_usbinitialize - * - * Description: - * Called to setup USB-related GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_usbinitialize(void) -{ - /* The OTG FS has an internal soft pull-up */ - - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ - -#ifdef CONFIG_STM32_OTGFS - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_VBUS); - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ -#endif -} - -/************************************************************************************ - * Name: stm32_usbsuspend - * - * Description: - * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is - * used. This function is called whenever the USB enters or leaves suspend mode. - * This is an opportunity for the board logic to shutdown clocks, power, etc. - * while the USB is suspended. - * - ************************************************************************************/ - -__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) -{ - uinfo("resume: %d\n", resume); -} diff --git a/src/drivers/uavcanesc/.gitignore b/src/drivers/uavcanesc/.gitignore deleted file mode 100644 index 24fbf171f0..0000000000 --- a/src/drivers/uavcanesc/.gitignore +++ /dev/null @@ -1 +0,0 @@ -./dsdlc_generated/ diff --git a/src/drivers/uavcanesc/CMakeLists.txt b/src/drivers/uavcanesc/CMakeLists.txt deleted file mode 100644 index c4677b742f..0000000000 --- a/src/drivers/uavcanesc/CMakeLists.txt +++ /dev/null @@ -1,98 +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. -# -############################################################################ - -set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) -set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) - -px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) - -set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") -set(UAVCAN_PLATFORM generic CACHE STRING "uavcan platform") - -string(TOUPPER "${PX4_PLATFORM}" OS_UPPER) -add_definitions( - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 - -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 - -DUAVCAN_NO_ASSERTIONS - -DUAVCAN_PLATFORM=generic - -DUAVCAN_STM32_${OS_UPPER}=1 - -DUAVCAN_STM32_NUM_IFACES=1 - -DUAVCAN_STM32_TIMER_NUMBER=2 - -DUAVCAN_USE_CPP03=ON - -DUAVCAN_USE_EXTERNAL_SNPRINT - ) - -add_compile_options(-Wno-cast-align) # TODO: fix and enable -add_subdirectory(${LIBUAVCAN_DIR} uavcanesc_libuavcan) -add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver uavcanesc_uavcan_drivers) -target_include_directories(uavcan_stm32_driver PUBLIC - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated - ) -add_dependencies(uavcan prebuild_targets) - -include_directories(${PX4_SOURCE_DIR}/src/modules/systemlib/flashparams) -include_directories(${LIBUAVCAN_DIR}/libuavcan/include) -include_directories(${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated) -include_directories(${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include) -include_directories(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver/include) - -include_directories(${PX4_SOURCE_DIR}/src/drivers/bootloaders) - -px4_add_module( - MODULE modules__uavcanesc - MAIN uavcanesc - STACK_MAIN 4096 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - uavcanesc_main.cpp - indication_controller.cpp - led.cpp - DEPENDS - drivers_bootloaders - git_uavcan - version - cdev - - uavcan_stm32_driver - - # within libuavcan - libuavcan_dsdlc - uavcan - ) - -add_subdirectory(commands) -add_subdirectory(nshterm) diff --git a/src/drivers/uavcanesc/commands/CMakeLists.txt b/src/drivers/uavcanesc/commands/CMakeLists.txt deleted file mode 100644 index b3fe8e80a4..0000000000 --- a/src/drivers/uavcanesc/commands/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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. -# -############################################################################ - -add_subdirectory(cfg) -add_subdirectory(dc) -add_subdirectory(rpm) -add_subdirectory(selftest) -add_subdirectory(stat) diff --git a/src/drivers/uavcanesc/commands/cfg/CMakeLists.txt b/src/drivers/uavcanesc/commands/cfg/CMakeLists.txt deleted file mode 100644 index ee31b76554..0000000000 --- a/src/drivers/uavcanesc/commands/cfg/CMakeLists.txt +++ /dev/null @@ -1,47 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__commands__cfg - MAIN cfg - STACK_MAIN 1024 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - esc_cfg.cpp - - DEPENDS - ) - -# diff --git a/src/drivers/uavcanesc/commands/cfg/esc_cfg.cpp b/src/drivers/uavcanesc/commands/cfg/esc_cfg.cpp deleted file mode 100644 index d4e0f77ab6..0000000000 --- a/src/drivers/uavcanesc/commands/cfg/esc_cfg.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 -#include -#include - -#include "esc_cfg.hpp" - - -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tcfg {yada|yada|yada}"); -} - -extern "C" __EXPORT int cfg_main(int argc, char *argv[]); -int cfg_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/commands/cfg/esc_cfg.hpp b/src/drivers/uavcanesc/commands/cfg/esc_cfg.hpp deleted file mode 100644 index df773e49f9..0000000000 --- a/src/drivers/uavcanesc/commands/cfg/esc_cfg.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ diff --git a/src/drivers/uavcanesc/commands/dc/CMakeLists.txt b/src/drivers/uavcanesc/commands/dc/CMakeLists.txt deleted file mode 100644 index 6e2dad1bbe..0000000000 --- a/src/drivers/uavcanesc/commands/dc/CMakeLists.txt +++ /dev/null @@ -1,47 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__commands__dc - MAIN dc - STACK_MAIN 1024 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - esc_dc.cpp - - DEPENDS - ) - -# diff --git a/src/drivers/uavcanesc/commands/dc/esc_dc.cpp b/src/drivers/uavcanesc/commands/dc/esc_dc.cpp deleted file mode 100644 index a4a0daa6a8..0000000000 --- a/src/drivers/uavcanesc/commands/dc/esc_dc.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 -#include -#include - -#include "esc_dc.hpp" - - -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tdc {yada|yada|yada}"); -} - -extern "C" __EXPORT int dc_main(int argc, char *argv[]); -int dc_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/commands/dc/esc_dc.hpp b/src/drivers/uavcanesc/commands/dc/esc_dc.hpp deleted file mode 100644 index df773e49f9..0000000000 --- a/src/drivers/uavcanesc/commands/dc/esc_dc.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ diff --git a/src/drivers/uavcanesc/commands/rpm/CMakeLists.txt b/src/drivers/uavcanesc/commands/rpm/CMakeLists.txt deleted file mode 100644 index b15b6d3605..0000000000 --- a/src/drivers/uavcanesc/commands/rpm/CMakeLists.txt +++ /dev/null @@ -1,47 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__commands__rpm - MAIN rpm - STACK_MAIN 1024 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - esc_rpm.cpp - - DEPENDS - ) - -# diff --git a/src/drivers/uavcanesc/commands/rpm/esc_rpm.cpp b/src/drivers/uavcanesc/commands/rpm/esc_rpm.cpp deleted file mode 100644 index eeedc37df0..0000000000 --- a/src/drivers/uavcanesc/commands/rpm/esc_rpm.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 -#include -#include - -#include "esc_rpm.hpp" - - -static void print_usage() -{ - PX4_INFO("usage: \n" - "\trpm {yada|yada|yada}"); -} - -extern "C" __EXPORT int rpm_main(int argc, char *argv[]); -int rpm_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/commands/rpm/esc_rpm.hpp b/src/drivers/uavcanesc/commands/rpm/esc_rpm.hpp deleted file mode 100644 index df773e49f9..0000000000 --- a/src/drivers/uavcanesc/commands/rpm/esc_rpm.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ diff --git a/src/drivers/uavcanesc/commands/selftest/CMakeLists.txt b/src/drivers/uavcanesc/commands/selftest/CMakeLists.txt deleted file mode 100644 index 22968c31fd..0000000000 --- a/src/drivers/uavcanesc/commands/selftest/CMakeLists.txt +++ /dev/null @@ -1,47 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__commands__selftest - MAIN selftest - STACK_MAIN 1024 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - esc_selftest.cpp - - DEPENDS - ) - -# diff --git a/src/drivers/uavcanesc/commands/selftest/esc_selftest.cpp b/src/drivers/uavcanesc/commands/selftest/esc_selftest.cpp deleted file mode 100644 index d5a992a5d7..0000000000 --- a/src/drivers/uavcanesc/commands/selftest/esc_selftest.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 -#include -#include - -#include "esc_selftest.hpp" - - -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tselftest {yada|yada|yada}"); -} - -extern "C" __EXPORT int selftest_main(int argc, char *argv[]); -int selftest_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/commands/selftest/esc_selftest.hpp b/src/drivers/uavcanesc/commands/selftest/esc_selftest.hpp deleted file mode 100644 index df773e49f9..0000000000 --- a/src/drivers/uavcanesc/commands/selftest/esc_selftest.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ diff --git a/src/drivers/uavcanesc/commands/stat/CMakeLists.txt b/src/drivers/uavcanesc/commands/stat/CMakeLists.txt deleted file mode 100644 index 74256c260c..0000000000 --- a/src/drivers/uavcanesc/commands/stat/CMakeLists.txt +++ /dev/null @@ -1,46 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__commands__stat - MAIN stat - STACK_MAIN 1024 - COMPILE_FLAGS - -Wno-deprecated-declarations - -O3 - SRCS - esc_stat.cpp - DEPENDS - ) - -# diff --git a/src/drivers/uavcanesc/commands/stat/esc_stat.cpp b/src/drivers/uavcanesc/commands/stat/esc_stat.cpp deleted file mode 100644 index f2e2698b0f..0000000000 --- a/src/drivers/uavcanesc/commands/stat/esc_stat.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 -#include -#include - -#include "esc_stat.hpp" - - -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tstat {yada|yada|yada}"); -} - -extern "C" __EXPORT int stat_main(int argc, char *argv[]); -int stat_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/commands/stat/esc_stat.hpp b/src/drivers/uavcanesc/commands/stat/esc_stat.hpp deleted file mode 100644 index df773e49f9..0000000000 --- a/src/drivers/uavcanesc/commands/stat/esc_stat.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ diff --git a/src/drivers/uavcanesc/indication_controller.cpp b/src/drivers/uavcanesc/indication_controller.cpp deleted file mode 100644 index 521a74103b..0000000000 --- a/src/drivers/uavcanesc/indication_controller.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko - * - * 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 "indication_controller.hpp" -#include -#include "led.hpp" - -namespace -{ -unsigned self_light_index = 0; - -void cb_light_command(const uavcan::ReceivedDataStructure &msg) -{ - uavcan::uint32_t red = 0; - uavcan::uint32_t green = 0; - uavcan::uint32_t blue = 0; - - for (auto &cmd : msg.commands) { - if (cmd.light_id == self_light_index) { - using uavcan::equipment::indication::RGB565; - - red = uavcan::uint32_t(float(cmd.color.red) * - (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F); - - green = uavcan::uint32_t(float(cmd.color.green) * - (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F); - - blue = uavcan::uint32_t(float(cmd.color.blue) * - (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F); - - red = uavcan::min(red, 0xFFU); - green = uavcan::min(green, 0xFFU); - blue = uavcan::min(blue, 0xFFU); - } - } -} -} - -int init_indication_controller(uavcan::INode &node) -{ - static uavcan::Subscriber sub_light(node); - - self_light_index = 0; - - int res = 0; - - res = sub_light.start(cb_light_command); - - if (res != 0) { - return res; - } - - return 0; -} diff --git a/src/drivers/uavcanesc/indication_controller.hpp b/src/drivers/uavcanesc/indication_controller.hpp deleted file mode 100644 index b8a33aa733..0000000000 --- a/src/drivers/uavcanesc/indication_controller.hpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#include - - -int init_indication_controller(uavcan::INode &node); diff --git a/src/drivers/uavcanesc/led.cpp b/src/drivers/uavcanesc/led.cpp deleted file mode 100644 index fcadc5c4ee..0000000000 --- a/src/drivers/uavcanesc/led.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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 - -#include -#include "hardware/stm32_tim.h" - - -#include "led.hpp" - -void rgb_led(int r, int g, int b, int freqs) -{ - - long fosc = 72000000; - long prescale = 2048; - long p1s = fosc / prescale; - long p0p5s = p1s / 2; - stm32_tim_channel_t mode = (stm32_tim_channel_t)(STM32_TIM_CH_OUTPWM | STM32_TIM_CH_POLARITY_NEG); - static struct stm32_tim_dev_s *tim = 0; - - if (tim == 0) { - tim = stm32_tim_init(3); - STM32_TIM_SETMODE(tim, STM32_TIM_MODE_UP); - STM32_TIM_SETCLOCK(tim, p1s - 8); - STM32_TIM_SETPERIOD(tim, p1s); - STM32_TIM_SETCOMPARE(tim, 1, 0); - STM32_TIM_SETCOMPARE(tim, 2, 0); - STM32_TIM_SETCOMPARE(tim, 3, 0); - STM32_TIM_SETCHANNEL(tim, 1, mode); - STM32_TIM_SETCHANNEL(tim, 2, mode); - STM32_TIM_SETCHANNEL(tim, 3, mode); - } - - long p = freqs == 0 ? p1s : p1s / freqs; - STM32_TIM_SETPERIOD(tim, p); - - p = freqs == 0 ? p1s + 1 : p0p5s / freqs; - - STM32_TIM_SETCOMPARE(tim, 1, (r * p) / 255); - STM32_TIM_SETCOMPARE(tim, 2, (g * p) / 255); - STM32_TIM_SETCOMPARE(tim, 3, (b * p) / 255); -} diff --git a/src/drivers/uavcanesc/led.hpp b/src/drivers/uavcanesc/led.hpp deleted file mode 100644 index b68e4aa70d..0000000000 --- a/src/drivers/uavcanesc/led.hpp +++ /dev/null @@ -1,37 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: David Sidrane - * - * 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. - * - ****************************************************************************/ - -__BEGIN_DECLS -void rgb_led(int r, int g, int b, int freqs); -__END_DECLS diff --git a/src/drivers/uavcanesc/nshterm/CMakeLists.txt b/src/drivers/uavcanesc/nshterm/CMakeLists.txt deleted file mode 100644 index bf87d034a1..0000000000 --- a/src/drivers/uavcanesc/nshterm/CMakeLists.txt +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -px4_add_module( - MODULE modules__uavcanesc__nshterm - MAIN nshterm - STACK_MAIN 1500 - COMPILE_FLAGS - -Wno-deprecated-declarations - SRCS - nshterm.c - PRIORITY "SCHED_PRIORITY_DEFAULT-30" - DEPENDS - ) diff --git a/src/drivers/uavcanesc/nshterm/nshterm.c b/src/drivers/uavcanesc/nshterm/nshterm.c deleted file mode 100644 index 707431522a..0000000000 --- a/src/drivers/uavcanesc/nshterm/nshterm.c +++ /dev/null @@ -1,128 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Andrew Tridgell - * - * 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 nshterm.c - * start a nsh terminal on a given port. This can be useful for error - * handling in startup scripts to start a nsh shell on /dev/ttyACM0 - * for diagnostics - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -__EXPORT int nshterm_main(int argc, char *argv[]); - -int -nshterm_main(int argc, char *argv[]) -{ - if (argc < 2) { - printf("Usage: nshterm \n"); - exit(1); - } - - unsigned retries = 0; - int fd = -1; - - /* back off 800 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 800U * 1000U) { - usleep(50000); - } - - /* try to bring up the console - stop doing so if the system gets armed */ - while (true) { - - /* the retries are to cope with the behaviour of /dev/ttyACM0 */ - /* which may not be ready immediately. */ - fd = open(argv[1], O_RDWR); - - if (fd != -1) { - break; - } - - usleep(100000); - retries++; - } - - if (fd == -1) { - perror(argv[1]); - exit(1); - } - - /* set up the serial port with output processing */ - - /* 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(fd, &uart_config)) < 0) { - warnx("ERR get config %s: %d\n", argv[1], termios_state); - close(fd); - return -1; - } - - /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST); - - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR set config %s\n", argv[1]); - close(fd); - return -1; - } - - /* setup standard file descriptors */ - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); - - nsh_consolemain(0, NULL); - - close(fd); - - return OK; -} diff --git a/src/drivers/uavcanesc/uavcan_driver.hpp b/src/drivers/uavcanesc/uavcan_driver.hpp deleted file mode 100644 index bd951b5e61..0000000000 --- a/src/drivers/uavcanesc/uavcan_driver.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018 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. - * - ****************************************************************************/ - -/** - * @author David Sidrane - */ - -#pragma once -#if defined(UAVCAN_KINETIS_NUTTX) -# include -#elif defined(UAVCAN_STM32_NUTTX) -# include -#else -# error "Unsupported driver" -#endif diff --git a/src/drivers/uavcanesc/uavcanesc_main.cpp b/src/drivers/uavcanesc/uavcanesc_main.cpp deleted file mode 100644 index 902cdb6f5a..0000000000 --- a/src/drivers/uavcanesc/uavcanesc_main.cpp +++ /dev/null @@ -1,512 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko - * David Sidrane - * - * 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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "uavcanesc_main.hpp" -#include "led.hpp" -#include "indication_controller.hpp" - -#include "boot_app_shared.h" - -using namespace time_literals; - -/** - * - * Implements basic functionality of UAVCAN esc. - */ - -/* - * This is the AppImageDescriptor used - * by the make_can_boot_descriptor.py tool to set - * the application image's descriptor so that the - * uavcan bootloader has the ability to validate the - * image crc, size etc of this application -*/ - -boot_app_shared_section app_descriptor_t AppDescriptor = { - .signature = {APP_DESCRIPTOR_SIGNATURE}, - .image_crc = 0, - .image_size = 0, - .vcs_commit = 0, - .major_version = APP_VERSION_MAJOR, - .minor_version = APP_VERSION_MINOR, - .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff } -}; - -/* - * UavcanNode - */ -UavcanEsc *UavcanEsc::_instance; - -UavcanEsc::UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : - CDev(UAVCAN_DEVICE_PATH), - active_bitrate(0), - _node(can_driver, system_clock), - _node_mutex(), - _fw_update_listner(_node), - _reset_timer(_node) -{ - const int res = pthread_mutex_init(&_node_mutex, nullptr); - - if (res < 0) { - std::abort(); - } - - px4_sem_init(&_sem, 0, 0); - /* semaphore use case is a signal */ - px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); - -} - -UavcanEsc::~UavcanEsc() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - - do { - /* wait 5ms - it should wake every 10ms or so worst-case */ - ::usleep(5000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - _instance = nullptr; - px4_sem_destroy(&_sem); -} - -int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate) -{ - - - if (_instance != nullptr) { - PX4_WARN("Already started"); - return -1; - } - - /* - * CAN driver init - */ - static CanInitHelper can; - static bool can_initialized = false; - - if (!can_initialized) { - const int can_init_res = can.init(bitrate); - - if (can_init_res < 0) { - PX4_WARN("CAN driver init failed %i", can_init_res); - return can_init_res; - } - - can_initialized = true; - } - - /* - * Node init - */ - _instance = new UavcanEsc(can.driver, uavcan_stm32::SystemClock::instance()); - - if (_instance == nullptr) { - PX4_WARN("Out of memory"); - return -1; - } - - - const int node_init_res = _instance->init(node_id, can.driver.updateEvent()); - - if (node_init_res < 0) { - delete _instance; - _instance = nullptr; - PX4_WARN("Node init failed %i", node_init_res); - return node_init_res; - } - - - /* Keep the bit rate for reboots on BenginFirmware updates */ - - _instance->active_bitrate = bitrate; - - /* - * Start the task. Normally it should never exit. - */ - static auto run_trampoline = [](int, char *[]) {return UavcanEsc::_instance->run();}; - _instance->_task = px4_task_spawn_cmd("uavcanesc", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, - static_cast(run_trampoline), nullptr); - - if (_instance->_task < 0) { - PX4_WARN("start failed: %d", errno); - return -errno; - } - - return OK; -} - -void UavcanEsc::fill_node_info() -{ - /* software version */ - uavcan::protocol::SoftwareVersion swver; - - // Extracting the first 8 hex digits of the git hash and converting them to int - char fw_git_short[9] = {}; - std::memmove(fw_git_short, px4_firmware_version_string(), 8); - char *end = nullptr; - swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; - swver.major = AppDescriptor.major_version; - swver.minor = AppDescriptor.minor_version; - swver.image_crc = AppDescriptor.image_crc; - - PX4_WARN("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); - - _node.setSoftwareVersion(swver); - - /* hardware version */ - uavcan::protocol::HardwareVersion hwver; - - hwver.major = HW_VERSION_MAJOR; - hwver.minor = HW_VERSION_MINOR; - - mfguid_t mfgid = {}; - board_get_mfguid(mfgid); - uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin()); - - _node.setHardwareVersion(hwver); -} - -static void cb_reboot(const uavcan::TimerEvent &) -{ - px4_systemreset(false); - -} - -void UavcanEsc::cb_beginfirmware_update(const uavcan::ReceivedDataStructure - &req, - uavcan::ServiceResponseDataStructure &rsp) -{ - static bool inprogress = false; - - rsp.error = rsp.ERROR_UNKNOWN; - - if (req.image_file_remote_path.path.size()) { - rsp.error = rsp.ERROR_IN_PROGRESS; - - if (!inprogress) { - inprogress = true; - bootloader_app_shared_t shared; - shared.bus_speed = active_bitrate; - shared.node_id = _node.getNodeID().get(); - bootloader_app_shared_write(&shared, App); - rgb_led(255, 128, 0, 5); - _reset_timer.setCallback(cb_reboot); - _reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000)); - rsp.error = rsp.ERROR_OK; - } - } -} - -int UavcanEsc::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events) -{ - int ret = -1; - - // Do regular cdev init - ret = CDev::init(); - - if (ret != OK) { - return ret; - } - - _node.setName(HW_UAVCAN_NAME); - - _node.setNodeID(node_id); - - fill_node_info(); - - const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this, - &UavcanEsc::cb_beginfirmware_update)); - - if (srv_start_res < 0) { - return ret; - } - - bus_events.registerSignalCallback(UavcanEsc::busevent_signal_trampoline); - - return _node.start(); -} - - -/* - * Restart handler - */ -class RestartRequestHandler: public uavcan::IRestartRequestHandler -{ - bool handleRestartRequest(uavcan::NodeID request_source) override - { - ::syslog(LOG_INFO, "UAVCAN: Restarting by request from %i\n", int(request_source.get())); - ::usleep(20 * 1000 * 1000); - px4_systemreset(false); - return true; // Will never be executed BTW - } -} restart_request_handler; - -void UavcanEsc::node_spin_once() -{ - const int spin_res = _node.spin(uavcan::MonotonicTime()); - - if (spin_res < 0) { - PX4_WARN("node spin error %i", spin_res); - } -} - -static void signal_callback(void *arg) -{ - /* Note: we are in IRQ context here */ - px4_sem_t *sem = (px4_sem_t *)arg; - int semaphore_value; - - if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) { - return; - } - - px4_sem_post(sem); -} - - -void -UavcanEsc::busevent_signal_trampoline() -{ - if (_instance) { - signal_callback(&_instance->_sem); - } -} - -int UavcanEsc::run() -{ - - get_node().setRestartRequestHandler(&restart_request_handler); - - while (init_indication_controller(get_node()) < 0) { - ::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n"); - ::sleep(1); - } - - (void)pthread_mutex_lock(&_node_mutex); - - _node.setModeOperational(); - - hrt_call timer_call{}; - hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem); - - while (!_task_should_exit) { - // Mutex is unlocked while the thread is blocked on IO multiplexing - (void)pthread_mutex_unlock(&_node_mutex); - - while (px4_sem_wait(&_sem) != 0); - - (void)pthread_mutex_lock(&_node_mutex); - - node_spin_once(); // Non-blocking - - // Do Something - - } - - hrt_cancel(&timer_call); - teardown(); - (void)pthread_mutex_unlock(&_node_mutex); - - exit(0); -} - -int -UavcanEsc::teardown() -{ - return 0; -} - - - -int -UavcanEsc::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - if (ret == -ENOTTY) { - ret = CDev::ioctl(filp, cmd, arg); - } - - return ret; -} - -void -UavcanEsc::print_info() -{ - if (!_instance) { - PX4_WARN("not running, start first"); - } - - (void)pthread_mutex_lock(&_node_mutex); - - - (void)pthread_mutex_unlock(&_node_mutex); -} - -/* - * App entry point - */ -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tuavcanesc {start|status|stop}"); -} - -extern "C" __EXPORT int uavcannode_start(int argc, char *argv[]); - -int uavcannode_start(int argc, char *argv[]) -{ - // CAN bitrate - int32_t bitrate = 0; - // Node ID - int32_t node_id = 0; - - // Did the bootloader auto baud and get a node ID Allocated - - bootloader_app_shared_t shared; - int valid = bootloader_app_shared_read(&shared, BootLoader); - - if (valid == 0) { - - bitrate = shared.bus_speed; - node_id = shared.node_id; - - // Invalidate to prevent deja vu - - bootloader_app_shared_invalidate(); - - } else { - - // Node ID - (void)param_get(param_find("ESC_NODE_ID"), &node_id); - (void)param_get(param_find("ESC_BITRATE"), &bitrate); - } - - if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { - PX4_WARN("Invalid Node ID %i", node_id); - ::exit(1); - } - - // Start - PX4_WARN("Node ID %u, bitrate %u", node_id, bitrate); - int rv = UavcanEsc::start(node_id, bitrate); - ::sleep(1); - return rv; -} - -extern "C" __EXPORT int uavcanesc_main(int argc, char *argv[]); -int uavcanesc_main(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - ::exit(1); - } - - if (!std::strcmp(argv[1], "start")) { - - if (UavcanEsc::instance()) { - errx(1, "already started"); - } - - return uavcannode_start(argc, argv); - } - - /* commands below require the app to be started */ - UavcanEsc *const inst = UavcanEsc::instance(); - - if (!inst) { - PX4_ERR("application not running"); - ::exit(1); - - } - - if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - inst->print_info(); - ::exit(0); - } - - if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); - } - - print_usage(); - ::exit(1); -} diff --git a/src/drivers/uavcanesc/uavcanesc_main.hpp b/src/drivers/uavcanesc/uavcanesc_main.hpp deleted file mode 100644 index 523c672e27..0000000000 --- a/src/drivers/uavcanesc/uavcanesc_main.hpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko - * David Sidrane - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -/** - * - * Defines functionality of UAVCAN ESC node. - */ - -#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1 -#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" - -// we add 1 to allow for busevent -#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+1) - -/** - * A UAVCAN node. - */ -class UavcanEsc : public cdev::CDev -{ - /* - * This memory is reserved for uavcan to use as over flow for message - * Coming from multiple sources that my not be considered at development - * time. - * - * The call to getNumFreeBlocks will tell how many blocks there are - * free -and multiply it times getBlockSize to get the number of bytes - * - */ - static constexpr unsigned MemPoolSize = 2048; - - /* - * This memory is reserved for uavcan to use for queuing CAN frames. - * At 1Mbit there is approximately one CAN frame every 200 uS. - * The number of buffers sets how long you can go without calling - * node_spin_xxxx. Since our task is the only one running and the - * driver will light the fd when there is a CAN frame we can nun with - * a minimum number of buffers to conserver memory. Each buffer is - * 32 bytes. So 5 buffers costs 160 bytes and gives us a maximum required - * poll rate of ~1 mS - * - */ - static constexpr unsigned RxQueueLenPerIface = 5; - - /* - * This memory is uses for the tasks stack size - */ - - static constexpr unsigned StackSize = 4096; - -public: - typedef uavcan::Node Node; - typedef uavcan_stm32::CanInitHelper CanInitHelper; - typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate; - - UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); - - virtual ~UavcanEsc(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - - static int start(uavcan::NodeID node_id, uint32_t bitrate); - - Node &get_node() { return _node; } - - int teardown(); - - void print_info(); - - static UavcanEsc *instance() { return _instance; } - - - /* The bit rate that can be passed back to the bootloader */ - - int32_t active_bitrate; - - -private: - void fill_node_info(); - int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events); - void node_spin_once(); - int run(); - static void busevent_signal_trampoline(); - - - px4_sem_t _sem; ///< semaphore for scheduling the task - int _task = -1; ///< handle to the OS task - bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver - - static UavcanEsc *_instance; ///< singleton pointer - Node _node; ///< library instance - pthread_mutex_t _node_mutex; - - typedef uavcan::MethodBinder &, - uavcan::ServiceResponseDataStructure &)> - BeginFirmwareUpdateCallBack; - - uavcan::ServiceServer _fw_update_listner; - void cb_beginfirmware_update(const uavcan::ReceivedDataStructure &req, - uavcan::ServiceResponseDataStructure &rsp); - -public: - - /* A timer used to reboot after the response is sent */ - - uavcan::TimerEventForwarder _reset_timer; - -}; diff --git a/src/drivers/uavcanesc/uavcanesc_params.c b/src/drivers/uavcanesc/uavcanesc_params.c deleted file mode 100644 index a0d7e55c03..0000000000 --- a/src/drivers/uavcanesc/uavcanesc_params.c +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 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. - * - ****************************************************************************/ - -/** - * @author Pavel Kirienko - */ - -#include -#include - -/** - * UAVCAN Node ID. - * - * Read the specs at http://uavcan.org to learn more about Node ID. - * - * @min 1 - * @max 125 - * @group UAVCAN - */ -PARAM_DEFINE_INT32(ESC_NODE_ID, 120); - -/** - * UAVCAN CAN bus bitrate. - * - * @min 20000 - * @max 1000000 - * @group UAVCAN - */ -PARAM_DEFINE_INT32(ESC_BITRATE, 1000000);