diff --git a/Images/px4-same70xplained-v1.prototype b/Images/px4-same70xplained-v1.prototype new file mode 100644 index 0000000000..84ba1fcecd --- /dev/null +++ b/Images/px4-same70xplained-v1.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 110, + "magic": "PX4FWv1", + "description": "Firmware for the SAME70xplained board", + "image": "", + "build_time": 0, + "summary": "PX4/SAME70xplained", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index 0cfb672d27..40bb156de0 100644 --- a/Makefile +++ b/Makefile @@ -211,6 +211,7 @@ misc_qgc_extra_firmware: \ # Other NuttX firmware alt_firmware: \ check_nxphlite-v3_default \ + check_px4-same70xplained-v1_default \ check_px4-stm32f4discovery_default \ check_px4cannode-v1_default \ check_px4esc-v1_default \ diff --git a/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake new file mode 100644 index 0000000000..96bf78ea3f --- /dev/null +++ b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake @@ -0,0 +1,200 @@ +include(nuttx/px4_impl_nuttx) + +px4_nuttx_configure(HWCLASS m7 CONFIG nsh ROMFS y ROMFSROOT px4fmu_common) + +set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +##set(config_uavcan_num_ifaces 2) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/samv7 +#WIP drivers/samv7/adc + drivers/samv7/tone_alarm + drivers/led + drivers/px4fmu +#WIP drivers/px4io + drivers/boards/px4-same70xplained-v1 + drivers/rgbled + drivers/mpu6000 + drivers/mpu9250 + drivers/lsm303d + drivers/l3gd20 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/ll40ls + drivers/trone + drivers/gps +#WIP drivers/pwm_out_sim + drivers/hott + drivers/hott/hott_telemetry + drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + drivers/oreoled +## drivers/gimbal +#WIP drivers/pwm_input +#WIP drivers/camera_trigger + drivers/bst + drivers/snapdragon_rc_pwm + drivers/lis3mdl + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib +#WIP systemcmds/hardfault_log + systemcmds/reboot + #systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm +# systemcmds/mtd Excluded until TWIHS works + systemcmds/dumpfile + systemcmds/ver + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led +##WIP modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/attitude_estimator_q + modules/position_estimator_inav + modules/local_position_estimator + modules/ekf2 + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 +## modules/logger + + # + # Library modules + # + modules/systemlib/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/dataman + + # + # Libraries + # + lib/controllib + lib/mathlib + lib/mathlib/math/filter + lib/rc + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/led + lib/DriverFramework/framework + lib/launchdetection + lib/version + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + #modules/bottle_drop + + # + # Rover apps + # + #examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + + set(config_extra_libs +# uavcan +# uavcan_stm32_driver + ) + +set(config_io_extra_libs + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "sercon" + STACK_MAIN "2048" + COMPILE_FLAGS "-Os") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "serdis" + STACK_MAIN "2048" + COMPILE_FLAGS "-Os") diff --git a/nuttx-configs/px4-same70xplained-v1/Kconfig b/nuttx-configs/px4-same70xplained-v1/Kconfig new file mode 100644 index 0000000000..42f0796535 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/Kconfig @@ -0,0 +1,22 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4_SAME70XPLAINED_V1 + +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-4 as PROBE_1-4 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided GPIO FMU-CH1-4 as PROBE_1-4 to provide timing signals from selected drivers" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-4 to provide timing signals from selected drivers. + +endif diff --git a/nuttx-configs/px4-same70xplained-v1/include/board.h b/nuttx-configs/px4-same70xplained-v1/include/board.h new file mode 100644 index 0000000000..37285013a1 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/include/board.h @@ -0,0 +1,443 @@ +/************************************************************************************ + * configs/same70-xplained/include/board.h + * + * 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. + * + ************************************************************************************/ + +#ifndef __CONFIGS_SAME70_XPLAINED_INCLUDE_BOARD_H +#define __CONFIGS_SAME70_XPLAINED_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Clocking *************************************************************************/ +/* After power-on reset, the SAME70Q device is running out of the Master Clock using + * the Fast RC Oscillator running at 4 MHz. + * + * MAINOSC: Frequency = 12MHz (crystal) + * + * 300MHz Settings: + * PLLA: PLL Divider = 1, Multiplier = 20 to generate PLLACK = 240MHz + * Master Clock (MCK): Source = PLLACK, Prescalar = 1 to generate MCK = 120MHz + * CPU clock: 120MHz + * + * There can be two on-board crystals. However, the the 32.768 crystal is not + * populated on the stock SAME70. The fallback is to use th on-chip, slow RC + * oscillator which has a frequency of 22-42 KHz, nominally 32 KHz. + */ + +#undef BOARD_HAVE_SLOWXTAL /* Slow crystal not populated */ +#define BOARD_SLOWCLK_FREQUENCY (32000) /* 32 KHz RC oscillator (nominal) */ +#define BOARD_MAINOSC_FREQUENCY (12000000) /* 12 MHz main oscillator */ + +/* Main oscillator register settings. + * + * The main oscillator could be either the embedded 4/8/12 MHz fast RC oscillators + * or an external 3-20 MHz crystal or ceramic resonator. The external clock source + * is selected by default in sam_clockconfig.c. Here we need to specify the main + * oscillator start-up time. + * + * REVISIT... this is old information: + * The start up time should be should be: + * + * Start Up Time = 8 * MOSCXTST / SLCK = 56 Slow Clock Cycles. + */ + +#define BOARD_CKGR_MOR_MOSCXTST (62 << PMC_CKGR_MOR_MOSCXTST_SHIFT) /* Start-up Time */ +#define BOARD_CKGR_MOR_MOSCXTENBY (PMC_CKGR_MOR_MOSCXTEN) /* Crystal Oscillator Enable */ + +/* PLLA configuration. + * + * Divider = 1 + * Multiplier = 25 + * + * Yields: + * + * PLLACK = 25 * 12MHz / 1 = 300MHz + */ + +#define BOARD_CKGR_PLLAR_STMODE PMC_CKGR_PLLAR_STMODE_FAST +#define BOARD_CKGR_PLLAR_COUNT (63 << PMC_CKGR_PLLAR_COUNT_SHIFT) +#define BOARD_CKGR_PLLAR_MUL PMC_CKGR_PLLAR_MUL(24) +#define BOARD_CKGR_PLLAR_DIV PMC_CKGR_PLLAR_DIV_BYPASS + +/* PMC master clock register settings. + * + * BOARD_PMC_MCKR_CSS - The source of main clock input. This may be one of: + * + * PMC_MCKR_CSS_SLOW Slow Clock + * PMC_MCKR_CSS_MAIN Main Clock + * PMC_MCKR_CSS_PLLA PLLA Clock + * PMC_MCKR_CSS_UPLL Divided UPLL Clock + * + * BOARD_PMC_MCKR_PRES - Source clock pre-scaler. May be one of: + * + * PMC_MCKR_PRES_DIV1 Selected clock + * PMC_MCKR_PRES_DIV2 Selected clock divided by 2 + * PMC_MCKR_PRES_DIV4 Selected clock divided by 4 + * PMC_MCKR_PRES_DIV8 Selected clock divided by 8 + * PMC_MCKR_PRES_DIV16 Selected clock divided by 16 + * PMC_MCKR_PRES_DIV32 Selected clock divided by 32 + * PMC_MCKR_PRES_DIV64 Selected clock divided by 64 + * PMC_MCKR_PRES_DIV3 Selected clock divided by 3 + * + * The prescaler determines (1) the CPU clock and (2) the input into the + * second divider that then generates the Master Clock (MCK). MCK is the + * source clock of the peripheral clocks. + * + * BOARD_PMC_MCKR_MDIV - MCK divider. May be one of: + * + * PMC_MCKR_MDIV_DIV1 Master Clock = Prescaler Output Clock / 1 + * PMC_MCKR_MDIV_DIV2 Master Clock = Prescaler Output Clock / 2 + * PMC_MCKR_MDIV_DIV4 Master Clock = Prescaler Output Clock / 4 + * PMC_MCKR_MDIV_DIV3 Master Clock = Prescaler Output Clock / 3 + */ + +#define BOARD_PMC_MCKR_CSS PMC_MCKR_CSS_PLLA /* Source = PLLA */ +#define BOARD_PMC_MCKR_PRES PMC_MCKR_PRES_DIV1 /* Prescaler = /1 */ +#define BOARD_PMC_MCKR_MDIV PMC_MCKR_MDIV_DIV2 /* MCK divider = /2 */ + +/* USB clocking */ + +#define BOARD_PMC_MCKR_UPLLDIV2 0 /* UPLL clock not divided by 2 */ + +/* Resulting frequencies */ + +#define BOARD_PLLA_FREQUENCY (300000000) /* PLLACK: 25 * 12Mhz / 1 */ +#define BOARD_CPU_FREQUENCY (300000000) /* CPU: PLLACK / 1 */ +#define BOARD_MCK_FREQUENCY (150000000) /* MCK: PLLACK / 1 / 2 */ +#undef BOARD_UPLL_FREQUENCY /* To be provided */ + +/* HSMCI clocking + * + * Multimedia Card Interface clock (MCCK or MCI_CK) is Master Clock (MCK) + * divided by (2*(CLKDIV) + CLOCKODD + 2). + * + * MCI_SPEED = MCK / (2*CLKDIV + CLOCKODD + 2) + * + * Where CLKDIV has a range of 0-255. + */ + +/* MCK = 150MHz, CLKDIV = 186, MCI_SPEED = 150MHz / (2*186 + 1 + 2) = 400 KHz */ + +#define HSMCI_INIT_CLKDIV ((186 << HSMCI_MR_CLKDIV_SHIFT) | HSMCI_MR_CLKODD) + +/* MCK = 150MHz, CLKDIV = 3 w/CLOCKODD, MCI_SPEED = 150MHz /(2*3 + 0 + 2) = 18.75 MHz */ + +#define HSMCI_MMCXFR_CLKDIV (2 << HSMCI_MR_CLKDIV_SHIFT) + +/* MCK = 150MHz, CLKDIV = 2, MCI_SPEED = 150MHz /(2*2 + 0 + 2) = 25 MHz */ + +#define HSMCI_SDXFR_CLKDIV (2 << HSMCI_MR_CLKDIV_SHIFT) +#define HSMCI_SDWIDEXFR_CLKDIV HSMCI_SDXFR_CLKDIV + +/* FLASH wait states. + * + * Wait states Max frequency at 105 centigrade (STH conditions) + * + * VDDIO + * 1.62V 2.7V + * --- ------- ------- + * 0 26 MHz 30 MHz + * 1 52 MHz 62 MHz + * 2 78 MHz 93 MHz + * 3 104 MHz 124 MHz + * 4 131 MHz 150 MHz + * 5 150 MHz --- MHz + * + * Given: VDDIO=3.3V, VDDCORE=1.2V, MCK=150MHz + */ + +#define BOARD_FWS 4 + +/* LED definitions ******************************************************************/ +/* LEDs + * + * A single LED is available driven by PC8. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED0 0 +#define BOARD_NLEDS 1 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED0_BIT (1 << BOARD_LED0) + +/* 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 defined in + * include/board.h and src/sam_autoleds.c. The LEDs are used to encode + * OS-related events as follows: + * + * ------------------- ---------------------------- ------ + * SYMBOL Meaning LED + * ------------------- ---------------------------- ------ */ + +#define LED_STARTED 0 /* NuttX has been started OFF */ +#define LED_HEAPALLOCATE 0 /* Heap has been allocated OFF */ +#define LED_IRQSENABLED 0 /* Interrupts enabled OFF */ +#define LED_STACKCREATED 1 /* Idle stack created ON */ +#define LED_INIRQ 2 /* In an interrupt N/C */ +#define LED_SIGNAL 2 /* In a signal handler N/C */ +#define LED_ASSERTION 2 /* An assertion failed N/C */ +#define LED_PANIC 3 /* The system has crashed FLASH */ +#undef LED_IDLE /* MCU is is sleep mode Not used */ + +/* Thus is LED is statically on, NuttX has successfully booted and is, + * apparently, running normally. If LED is flashing at approximately + * 2Hz, then a fatal error has been detected and the system has halted. + */ + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN19) +# define PROBE_2 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN26) +# define PROBE_3 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOA | GPIO_PIN2) +# define PROBE_4 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOA | GPIO_PIN24) + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { sam_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { sam_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { sam_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { sam_configgpio(PROBE_4); } \ + } while(0) + +# define PROBE(n,s) do {sam_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 + +/* Button definitions ***************************************************************/ +/* Buttons + * + * SAM E70 Xplained contains two mechanical buttons. One button is the RESET + * button connected to the SAM E70 reset line and the other, PA11, is a generic + * user configurable button. When a button is pressed it will drive the I/O + * line to GND. + * + * NOTE: There are no pull-up resistors connected to the generic user buttons + * so it is necessary to enable the internal pull-up in the SAM E70 to use the + * button. + */ + +#define BUTTON_SW0 0 +#define NUM_BUTTONS 1 + +#define BUTTON_SW0_BIT (1 << BUTTON_SW0) + +/* PIO Disambiguation ***************************************************************/ +/* Serial Console + * + * The SAME70-XPLD has no on-board RS-232 drivers so it will be necessary to use + * either the VCOM or an external RS-232 driver. Here are some options. + * + * - Arduino Serial Shield: One option is to use an Arduino-compatible + * serial shield. This will use the RXD and TXD signals available at pins + * 0 an 1, respectively, of the Arduino "Digital Low" connector. On the + * SAME70-XPLD board, this corresponds to UART3: + * + * ------ ------ ------- ------- -------- + * Pin on SAME70 Arduino Arduino SAME70 + * J503 PIO Name Pin Function + * ------ ------ ------- ------- -------- + * 1 PD28 RX0 0 URXD3 + * 2 PD30 TX0 1 UTXD3 + * ------ ------ ------- ------- -------- + * + * There are alternative pin selections only for UART3 TXD: + */ + +//#define GPIO_UART3_TXD GPIO_UART3_TXD_1 + + +/* - Arduino Communications. Additional UART/USART connections are available + * on the Arduino Communications connection J505: + * + * ------ ------ ------- ------- -------- + * Pin on SAME70 Arduino Arduino SAME70 + * J503 PIO Name Pin Function + * ------ ------ ------- ------- -------- + * 3 PD18 RX1 0 URXD4 + * 4 PD19 TX1 0 UTXD4 + * 5 PD15 RX2 0 RXD2 + * 6 PD16 TX2 0 TXD2 + * 7 PB0 RX3 0 RXD0 + * 8 PB1 TX3 1 TXD0 + * ------ ------ ------- ------- -------- + * + * There are alternative pin selections only for UART4 TXD: + */ + +//#define GPIO_UART4_TXD GPIO_UART4_TXD_1 + +/* - SAMV7-XULT EXTn connectors. USART pins are also available the EXTn + * connectors. The following are labelled in the User Guide for USART + * functionality: + * + * ---- -------- ------ -------- + * EXT1 EXTI1 SAME70 SAME70 + * Pin Name PIO Function + * ---- -------- ------ -------- + * 13 USART_RX PB00 RXD0 + * 14 USART_TX PB01 TXD0 + * + * ---- -------- ------ -------- + * EXT2 EXTI2 SAME70 SAME70 + * Pin Name PIO Function + * ---- -------- ------ -------- + * 13 USART_RX PA21 RXD1 + * 14 USART_TX PB04 TXD1 + * + * There are no alternative pin selections for USART0 or USART1. + */ + +/* - VCOM. The Virtual Com Port gateway is available on USART1: + * + * ------ -------- + * SAME70 SAME70 + * PIO Function + * ------ -------- + * PB04 TXD1 + * PA21 RXD1 + * ------ -------- + * + * There are no alternative pin selections for USART1. + */ + + +/* SAME70-XPLD board, this corresponds to UART1: + * + * ------ ------ ------- ------- -------- + * Pin on SAME70 SAME70 + * PIO Function + * ------ ------ ------- ------- -------- + * J503-3 PA5 URXD1 + * J503-4 PA6 UTXD1 + * ------ ------ ------- ------- -------- + * + * There are alternative pin selections only for UART1 TXD: + * + */ + +#define GPIO_UART1_TXD GPIO_UART1_TXD_3 + +/* SAME70-XPLD board, this corresponds to UART2: + * + * ------ ------ ------- ------- -------- + * Pin on SAME70 SAME70 + * PIO Function + * ------ ------ ------- ------- -------- + * J500-3 PD25 URXD2 + * J502-1 PD26 UTXD3 + * ------ ------ ------- ------- -------- + * + * There are No alternative pin selections only for UART2 + * + */ + + +/* MCAN1 + * + * SAM E70 Xplained has two MCAN modules that performs communication according + * to ISO11898-1 (Bosch CAN specification 2.0 part A,B) and Bosch CAN FD + * specification V1.0. MCAN1 is connected to an on-board ATA6561 CAN physical-layer + * transceiver. + * + * ------- -------- -------- ------------- + * SAM E70 FUNCTION ATA6561 SHARED + * PIN FUNCTION FUNCTIONALITY + * ------- -------- -------- ------------- + * PC14 CANTX1 TXD Shield + * PC12 CANRX1 RXD Shield + * ------- -------- -------- ------------- + */ + +#define GPIO_MCAN1_TX GPIO_MCAN1_TX_2 +#define GPIO_MCAN1_RX GPIO_MCAN1_RX_2 + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: sam_lcdclear + * + * Description: + * This is a non-standard LCD interface just for the SAM4e-EK board. Because + * of the various rotations, clearing the display in the normal way by writing a + * sequences of runs that covers the entire display can be very slow. Here the + * display is cleared by simply setting all GRAM memory to the specified color. + * + ************************************************************************************/ + +void sam_lcdclear(uint16_t color); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIGS_SAME70_XPLAINED_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4-same70xplained-v1/include/nsh_romfsimg.h b/nuttx-configs/px4-same70xplained-v1/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4-same70xplained-v1/nsh/Make.defs b/nuttx-configs/px4-same70xplained-v1/nsh/Make.defs new file mode 100644 index 0000000000..7000d1a1ba --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/nsh/Make.defs @@ -0,0 +1,161 @@ +############################################################################ +# configs/same70-xplained/nsh/Make.defs +# +# 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 +endif + +ifeq ($(CONFIG_ARMV7M_DTCM),y) + LDSCRIPT = flash-dtcm.ld +else + LDSCRIPT = ld.script +endif + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mkwindeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT) + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) +endif + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + ARCHOPTIMIZATION += -g +endif + +ifneq ($(CONFIG_DEBUG_NOOPT),y) + ARCHOPTIMIZATION += $(MAXOPTIMIZATION) +endif + +ARCHCFLAGS = -fno-builtin +ARCHCFLAGS += -std=gnu99 + +ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow -Wundef -fno-strict-aliasing +ARCHWARNINGS += -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs +ARCHWARNINGSXX = -Wall -Wshadow -Wundef +ARCHWARNINGSXX += $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# This seems to be the only way to add linker flags + +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +ASMEXT = .S +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +ifneq ($(CROSSDEV),arm-nuttx-elf-) + LDFLAGS += -nostartfiles -nodefaultlibs +endif +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + LDFLAGS += -g +endif + + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -Wundef -g -pipe +HOSTLDFLAGS = diff --git a/nuttx-configs/px4-same70xplained-v1/nsh/defconfig b/nuttx-configs/px4-same70xplained-v1/nsh/defconfig new file mode 100644 index 0000000000..bad6285759 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/nsh/defconfig @@ -0,0 +1,1353 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +CONFIG_DEFAULT_SMALL=y +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y +# CONFIG_UBOOT_UIMAGE is not set +# CONFIG_DFU_BINARY is not set + +# +# Customize Header Files +# +# CONFIG_ARCH_STDINT_H is not set +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set +# CONFIG_ARCH_DEBUG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG_ALERT=y +# CONFIG_DEBUG_FEATURES is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_STACK_COLORATION=y +# CONFIG_ARCH_HAVE_HEAPCHECK is not set +CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +# CONFIG_DEBUG_NOOPT is not set +# CONFIG_DEBUG_CUSTOMOPT is not set +CONFIG_DEBUG_FULLOPT=y + +# +# System Type +# +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_MISOC is not set +# CONFIG_ARCH_RENESAS is not set +# CONFIG_ARCH_RISCV is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_XTENSA is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_A1X is not set +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set +# CONFIG_ARCH_CHIP_IMX1 is not set +# CONFIG_ARCH_CHIP_IMX6 is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set +# CONFIG_ARCH_CHIP_LPC11XX is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_MOXART is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set +# CONFIG_ARCH_CHIP_SAML is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_SAMV7=y +# CONFIG_ARCH_CHIP_STM32 is not set +# CONFIG_ARCH_CHIP_STM32F0 is not set +# CONFIG_ARCH_CHIP_STM32F7 is not set +# CONFIG_ARCH_CHIP_STM32L4 is not set +# CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_CHIP_TMS570 is not set +# CONFIG_ARCH_CHIP_XMC4 is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +# CONFIG_ARCH_CORTEXM23 is not set +# CONFIG_ARCH_CORTEXM3 is not set +# CONFIG_ARCH_CORTEXM33 is not set +# CONFIG_ARCH_CORTEXM4 is not set +CONFIG_ARCH_CORTEXM7=y +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set +# CONFIG_ARCH_CORTEXA9 is not set +# CONFIG_ARCH_CORTEXR4 is not set +# CONFIG_ARCH_CORTEXR4F is not set +# CONFIG_ARCH_CORTEXR5 is not set +# CONFIG_ARCH_CORTEX5F is not set +# CONFIG_ARCH_CORTEXR7 is not set +# CONFIG_ARCH_CORTEXR7F is not set +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="samv7" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARMV7M_LAZYFPU is not set +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_HAVE_DPFPU=y +CONFIG_ARCH_FPU=y +# CONFIG_ARCH_DPFPU is not set +# CONFIG_ARCH_HAVE_TRUSTZONE is not set +CONFIG_ARM_HAVE_MPU_UNIFIED=y +# CONFIG_ARM_MPU is not set + +# +# ARMV7M Configuration Options +# +CONFIG_ARMV7M_HAVE_ICACHE=y +CONFIG_ARMV7M_HAVE_DCACHE=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y +CONFIG_ARMV7M_HAVE_ITCM=y +CONFIG_ARMV7M_HAVE_DTCM=y +# CONFIG_ARMV7M_ITCM is not set +# CONFIG_ARMV7M_DTCM is not set +# CONFIG_ARMV7M_TOOLCHAIN_IARL is not set +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y +CONFIG_ARMV7M_HAVE_STACKCHECK=y +# CONFIG_ARMV7M_STACKCHECK is not set +# CONFIG_ARMV7M_ITMSYSLOG is not set +CONFIG_SERIAL_TERMIOS=y + +# +# SAMV7 Configuration Options +# +# CONFIG_ARCH_CHIP_SAME70Q19 is not set +# CONFIG_ARCH_CHIP_SAME70Q20 is not set +CONFIG_ARCH_CHIP_SAME70Q21=y +# CONFIG_ARCH_CHIP_SAME70N19 is not set +# CONFIG_ARCH_CHIP_SAME70N20 is not set +# CONFIG_ARCH_CHIP_SAME70N21 is not set +# CONFIG_ARCH_CHIP_SAME70J19 is not set +# CONFIG_ARCH_CHIP_SAME70J20 is not set +# CONFIG_ARCH_CHIP_SAME70J21 is not set +# CONFIG_ARCH_CHIP_SAMV71Q19 is not set +# CONFIG_ARCH_CHIP_SAMV71Q20 is not set +# CONFIG_ARCH_CHIP_SAMV71Q21 is not set +# CONFIG_ARCH_CHIP_SAMV71N19 is not set +# CONFIG_ARCH_CHIP_SAMV71N20 is not set +# CONFIG_ARCH_CHIP_SAMV71N21 is not set +# CONFIG_ARCH_CHIP_SAMV71J19 is not set +# CONFIG_ARCH_CHIP_SAMV71J20 is not set +# CONFIG_ARCH_CHIP_SAMV71J21 is not set +CONFIG_ARCH_CHIP_SAME70=y +CONFIG_ARCH_CHIP_SAME70Q=y +# CONFIG_ARCH_CHIP_SAME70N is not set +# CONFIG_ARCH_CHIP_SAME70J is not set +# CONFIG_ARCH_CHIP_SAMV71 is not set +# CONFIG_ARCH_CHIP_SAMV71Q is not set +# CONFIG_ARCH_CHIP_SAMV71N is not set +# CONFIG_ARCH_CHIP_SAMV71J is not set +# CONFIG_SAMV7_MCAN is not set +CONFIG_SAMV7_HAVE_MCAN1=y +# CONFIG_SAMV7_DAC is not set +CONFIG_SAMV7_HAVE_DAC1=y +CONFIG_SAMV7_HAVE_EBI=y +# CONFIG_SAMV7_EMAC is not set +CONFIG_SAMV7_HSMCI=y +CONFIG_SAMV7_HAVE_HSMCI0=y +# CONFIG_SAMV7_HAVE_ISI8 is not set +# CONFIG_SAMV7_HAVE_MEDIALB is not set +CONFIG_SAMV7_HAVE_SDRAMC=y +CONFIG_SAMV7_HAVE_SPI0=y +CONFIG_SAMV7_HAVE_SPI1=y +# CONFIG_SAMV7_QSPI_IS_SPI is not set +# CONFIG_SAMV7_SSC is not set +CONFIG_SAMV7_HAVE_TC=y +CONFIG_SAMV7_HAVE_TWIHS2=y +# CONFIG_SAMV7_HAVE_USBFS is not set +CONFIG_SAMV7_HAVE_USBHS=y +CONFIG_SAMV7_HAVE_USART0=y +CONFIG_SAMV7_HAVE_USART1=y +CONFIG_SAMV7_HAVE_USART2=y +CONFIG_SAMV7_SPI=y +CONFIG_SAMV7_SPI_MASTER=y +# CONFIG_SAMV7_SPI_SLAVE is not set + +# +# SAMV7 Peripheral Selection +# +# CONFIG_SAMV7_ACC is not set +# CONFIG_SAMV7_ADC is not set +# CONFIG_SAMV7_AFEC0 is not set +# CONFIG_SAMV7_AFEC1 is not set +# CONFIG_SAMV7_MCAN0 is not set +# CONFIG_SAMV7_MCAN1 is not set +# CONFIG_SAMV7_DAC0 is not set +# CONFIG_SAMV7_DAC1 is not set +# CONFIG_SAMV7_EBI is not set +# CONFIG_SAMV7_EMAC0 is not set +CONFIG_SAMV7_XDMAC=y +CONFIG_SAMV7_HSMCI0=y +# CONFIG_SAMV7_ISI is not set +# CONFIG_SAMV7_PWM0 is not set +# CONFIG_SAMV7_PWM1 is not set +# CONFIG_SAMV7_QSPI is not set +CONFIG_SAMV7_RTC=y +# CONFIG_SAMV7_RTT is not set +CONFIG_SAMV7_SDRAMC=y +# CONFIG_SAMV7_SMC is not set +CONFIG_SAMV7_SPI0=y +# CONFIG_SAMV7_SPI1 is not set +# CONFIG_SAMV7_SSC0 is not set +CONFIG_SAMV7_TC0=y +CONFIG_SAMV7_TC1=y +CONFIG_SAMV7_TC2=y +CONFIG_SAMV7_TC3=y +# CONFIG_SAMV7_TRNG is not set +CONFIG_SAMV7_TWIHS0=y +# CONFIG_SAMV7_TWIHS1 is not set +# CONFIG_SAMV7_TWIHS2 is not set +# CONFIG_SAMV7_UART0 is not set +# CONFIG_SAMV7_UART1 is not set +# CONFIG_SAMV7_UART2 is not set +# CONFIG_SAMV7_UART3 is not set +# CONFIG_SAMV7_UART4 is not set +CONFIG_SAMV7_USBDEVHS=y +# CONFIG_SAMV7_USBHOSTHS is not set +CONFIG_SAMV7_USART0=y +CONFIG_SAMV7_USART1=y +# CONFIG_SAMV7_USART2 is not set +# CONFIG_SAMV7_WDT is not set +# CONFIG_SAMV7_RSWDT is not set +# CONFIG_SAMV7_JTAG_DISABLE is not set +CONFIG_SAMV7_JTAG_FULL_ENABLE=y +# CONFIG_SAMV7_JTAG_FULL_SW_ENABLE is not set +# CONFIG_SAMV7_JTAG_SW_ENABLE is not set +# CONFIG_SAMV7_ERASE_DISABLE is not set +CONFIG_SAMV7_ERASE_ENABLE=y +CONFIG_SAMV7_SYSTEMRESET=y +CONFIG_SAMV7_EXTRESET_ERST=0 +CONFIG_SAMV7_GPIO_IRQ=y +CONFIG_SAMV7_GPIOA_IRQ=y +# CONFIG_SAMV7_GPIOB_IRQ is not set +CONFIG_SAMV7_GPIOC_IRQ=y +# CONFIG_SAMV7_GPIOD_IRQ is not set +# CONFIG_SAMV7_GPIOE_IRQ is not set +# CONFIG_SAMV7_PROGMEM is not set + +# +# SDRAM Configuration +# +CONFIG_SAMV7_SDRAMSIZE=2097152 +# CONFIG_SAMV7_SDRAMHEAP is not set + +# +# SPI Device Driver Configuration +# +CONFIG_SAMV7_SPI0_MASTER=y + +# +# SPI Master Configuration +# +# CONFIG_SAMV7_SPI_CS_DECODING is not set +# CONFIG_SAMV7_SPI_VARSELECT is not set +# CONFIG_SAMV7_SPI_DMA is not set + +# +# TWIHS device driver options +# +CONFIG_SAMV7_TWIHS0_FREQUENCY=100000 +CONFIG_SAMV7_TWIHS0_GLITCH_FILTER=1 +CONFIG_SAMV7_TWIHS0_SINGLE_MASTER=y + +# +# Timer/counter Configuration +# +# CONFIG_SAMV7_TC0_CLK0 is not set +# CONFIG_SAMV7_TC0_TIOA0 is not set +# CONFIG_SAMV7_TC0_TIOB0 is not set +# CONFIG_SAMV7_TC0_CLK1 is not set +# CONFIG_SAMV7_TC0_TIOA1 is not set +# CONFIG_SAMV7_TC0_TIOB1 is not set +# CONFIG_SAMV7_TC0_CLK2 is not set +# CONFIG_SAMV7_TC0_TIOA2 is not set +# CONFIG_SAMV7_TC0_TIOB2 is not set +# CONFIG_SAMV7_TC1_CLK3 is not set +# CONFIG_SAMV7_TC1_TIOA3 is not set +# CONFIG_SAMV7_TC1_TIOB3 is not set +# CONFIG_SAMV7_TC1_CLK4 is not set +# CONFIG_SAMV7_TC1_TIOA4 is not set +# CONFIG_SAMV7_TC1_TIOB4 is not set +# CONFIG_SAMV7_TC1_CLK5 is not set +# CONFIG_SAMV7_TC1_TIOA5 is not set +# CONFIG_SAMV7_TC1_TIOB5 is not set +# CONFIG_SAMV7_TC2_CLK6 is not set +# CONFIG_SAMV7_TC2_TIOA6 is not set +# CONFIG_SAMV7_TC2_TIOB6 is not set +# CONFIG_SAMV7_TC2_CLK7 is not set +# CONFIG_SAMV7_TC2_TIOA7 is not set +# CONFIG_SAMV7_TC2_TIOB7 is not set +# CONFIG_SAMV7_TC2_CLK8 is not set +# CONFIG_SAMV7_TC2_TIOA8 is not set +# CONFIG_SAMV7_TC2_TIOB8 is not set +# CONFIG_SAMV7_TC3_CLK9 is not set +# CONFIG_SAMV7_TC3_TIOA9 is not set +# CONFIG_SAMV7_TC3_TIOB9 is not set +# CONFIG_SAMV7_TC3_CLK10 is not set +# CONFIG_SAMV7_TC3_TIOA10 is not set +# CONFIG_SAMV7_TC3_TIOB10 is not set +# CONFIG_SAMV7_TC3_CLK11 is not set +# CONFIG_SAMV7_TC3_TIOA11 is not set +# CONFIG_SAMV7_TC3_TIOB11 is not set +# CONFIG_SAMV7_ONESHOT is not set +# CONFIG_SAMV7_FREERUN is not set + +# +# HSMCI device driver options +# +CONFIG_SAMV7_HSMCI_DMA=y +# CONFIG_SAMV7_HSMCI_RDPROOF is not set +# CONFIG_SAMV7_HSMCI_WRPROOF is not set +# CONFIG_SAMV7_HSMCI_UNALIGNED is not set + +# +# USB High Speed Device Controller driver (DCD) options +# +# CONFIG_SAMV7_USBDEVHS_LOWPOWER is not set +CONFIG_SAMV7_USBHS_NDTDS=8 +CONFIG_SAMV7_USBHS_PREALLOCATE=y +CONFIG_SAMV7_USBHS_EP7DMA_WAR=y +# CONFIG_ARCH_TOOLCHAIN_IAR is not set +CONFIG_ARCH_TOOLCHAIN_GNU=y + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set +# CONFIG_ARCH_HAVE_MULTICPU is not set +CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_HAVE_POWEROFF is not set +CONFIG_ARCH_HAVE_RESET=y +# CONFIG_ARCH_HAVE_RTC_SUBSECONDS is not set +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set +CONFIG_ARCH_HAVE_RAMFUNCS=y +# CONFIG_ARCH_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set +# CONFIG_ARCH_MINIMAL_VECTORTABLE is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=51262 +# CONFIG_ARCH_CALIBRATION is not set + +# +# Interrupt options +# +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4048 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Boot Memory Configuration +# +CONFIG_RAM_START=0x20400000 +CONFIG_RAM_SIZE=393216 +# CONFIG_ARCH_HAVE_SDRAM is not set + +# +# Board Selection +# +# CONFIG_ARCH_BOARD_SAME70_XPLAINED is not set +CONFIG_ARCH_BOARD_PX4_SAME70XPLAINED_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4-same70xplained-v1" + +# +# Common Board Options +# + +# +# Board-Specific Options +# +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_RESET_ON_CRASH=y +CONFIG_LIB_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +# CONFIG_BOARDCTL_UNIQUEID is not set +CONFIG_BOARDCTL_USBDEVCTRL=y +# CONFIG_BOARDCTL_TSCTEST is not set +# CONFIG_BOARDCTL_GRAPHICS is not set +# CONFIG_BOARDCTL_IOCTL is not set + +# +# RTOS Features +# +# CONFIG_DISABLE_OS_API is not set + +# +# Clocks and Timers +# +CONFIG_START_YEAR=2016 +CONFIG_START_MONTH=11 +CONFIG_START_DAY=30 +CONFIG_ARCH_HAVE_TICKLESS=y +# CONFIG_SCHED_TICKLESS is not set +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set +# CONFIG_ARCH_HAVE_TIMEKEEPING is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=2016 +CONFIG_START_MONTH=11 +CONFIG_START_DAY=30 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_WDOG_INTRESERVE=4 +CONFIG_PREALLOC_TIMERS=50 + +# +# Tasks and Scheduling +# +# CONFIG_SPINLOCK is not set +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_RR_INTERVAL=0 +# CONFIG_SCHED_SPORADIC is not set +CONFIG_TASK_NAME_SIZE=24 +CONFIG_MAX_TASKS=32 +# CONFIG_SCHED_HAVE_PARENT is not set +CONFIG_SCHED_WAITPID=y + +# +# Pthread Options +# +# CONFIG_PTHREAD_MUTEX_TYPES is not set +CONFIG_PTHREAD_MUTEX_ROBUST=y +# CONFIG_PTHREAD_MUTEX_UNSAFE is not set +# CONFIG_PTHREAD_MUTEX_BOTH is not set +CONFIG_NPTHREAD_KEYS=4 +# CONFIG_PTHREAD_CLEANUP is not set +# CONFIG_CANCELLATION_POINTS is not set + +# +# Performance Monitoring +# +# CONFIG_SCHED_CPULOAD is not set +CONFIG_SCHED_INSTRUMENTATION=y +# CONFIG_SCHED_INSTRUMENTATION_PREEMPTION is not set +# CONFIG_SCHED_INSTRUMENTATION_CSECTION is not set +# CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS is not set +# CONFIG_SCHED_INSTRUMENTATION_BUFFER is not set + +# +# Files and I/O +# +CONFIG_DEV_CONSOLE=y +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NFILE_DESCRIPTORS=53 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 + +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +# CONFIG_SIG_EVTHREAD is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# POSIX Message Queue Options +# +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +# CONFIG_MODULE is not set + +# +# Work queue support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=192 +CONFIG_SCHED_HPWORKPERIOD=5000 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPNTHREADS=1 +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPRIOMAX=176 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=500 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +# CONFIG_LIB_SYSCALL is not set + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_DEV_URANDOM is not set +# CONFIG_DEV_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set +# CONFIG_ARCH_HAVE_PWM_MULTICHAN is not set +# CONFIG_PWM is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +# CONFIG_I2C_POLLED is not set +CONFIG_I2C_RESET=y +# CONFIG_I2C_TRACE is not set +# CONFIG_I2C_DRIVER is not set +# CONFIG_ARCH_HAVE_SPI_CRCGENERATION is not set +CONFIG_ARCH_HAVE_SPI_CS_CONTROL=y +# CONFIG_ARCH_HAVE_SPI_BITORDER is not set +CONFIG_SPI=y +# CONFIG_SPI_SLAVE is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_SPI_CALLBACK is not set +# CONFIG_SPI_HWFEATURES is not set +# CONFIG_SPI_CS_CONTROL is not set +# CONFIG_SPI_CS_DELAY_CONTROL is not set +# CONFIG_SPI_DRIVER is not set +# CONFIG_SPI_BITBANG is not set +# CONFIG_I2S is not set + +# +# Timer Driver Support +# +# CONFIG_TIMER is not set +# CONFIG_ONESHOT is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +# CONFIG_RTC_ALARM is not set +# CONFIG_RTC_DRIVER is not set +# CONFIG_RTC_EXTERNAL is not set +CONFIG_WATCHDOG=y +CONFIG_WATCHDOG_DEVPATH="/dev/watchdog0" +# CONFIG_TIMERS_CS2100CP is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set + +# +# IO Expander/GPIO Support +# +# CONFIG_IOEXPANDER is not set +# CONFIG_DEV_GPIO is not set + +# +# LCD Driver Support +# +# CONFIG_LCD is not set +# CONFIG_SLCD is not set + +# +# LED Support +# +# CONFIG_RGBLED is not set +# CONFIG_PCA9635PW is not set +# CONFIG_NCP5623C is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +CONFIG_MMCSD_HAVECARDDETECT=y +# CONFIG_MMCSD_SPI is not set +CONFIG_ARCH_HAVE_SDIO=y +CONFIG_SDIO_DMA=y +# CONFIG_ARCH_HAVE_SDIOWAIT_WRCOMPLETE is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_PREFLIGHT is not set +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_WIDTH_D1_ONLY is not set +CONFIG_SDIO_BLOCKSETUP=y +# CONFIG_MODEM is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +# CONFIG_MTD_SECT512 is not set +# CONFIG_MTD_PARTITION_NAMES is not set +CONFIG_MTD_BYTE_WRITE=y +# CONFIG_MTD_PROGMEM is not set +CONFIG_MTD_CONFIG=y +# CONFIG_MTD_CONFIG_RAM_CONSOLIDATE is not set +CONFIG_MTD_CONFIG_ERASEDVALUE=0xff + +# +# MTD Device Drivers +# +# CONFIG_MTD_NAND is not set +# CONFIG_RAMMTD is not set +# CONFIG_FILEMTD is not set +CONFIG_MTD_AT24XX=y +# CONFIG_AT24XX_MULTI is not set +CONFIG_AT24XX_SIZE=2 +CONFIG_AT24XX_ADDR=0x57 +CONFIG_AT24XX_EXTENDED=y +CONFIG_AT24XX_EXTSIZE=160 +CONFIG_AT24XX_FREQUENCY=100000 +# CONFIG_MTD_AT25 is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_IS25XP is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_MX25L is not set +# CONFIG_MTD_S25FL1 is not set +# CONFIG_MTD_N25QXXX is not set +# CONFIG_MTD_SMART is not set +# CONFIG_MTD_RAMTRON is not set +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST25XX is not set +# CONFIG_MTD_SST26 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +# CONFIG_EEPROM is not set +CONFIG_PIPES=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DEV_FIFO_SIZE=70 +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +CONFIG_SERIAL_CONSOLE=y +# CONFIG_16550_UART is not set +# CONFIG_UART_SERIALDRIVER is not set +# CONFIG_UART0_SERIALDRIVER is not set +# CONFIG_UART1_SERIALDRIVER is not set +# CONFIG_UART2_SERIALDRIVER is not set +# CONFIG_UART3_SERIALDRIVER is not set +# CONFIG_UART4_SERIALDRIVER is not set +# CONFIG_UART5_SERIALDRIVER is not set +# CONFIG_UART6_SERIALDRIVER is not set +# CONFIG_UART7_SERIALDRIVER is not set +# CONFIG_UART8_SERIALDRIVER is not set +# CONFIG_SCI0_SERIALDRIVER is not set +# CONFIG_SCI1_SERIALDRIVER is not set +CONFIG_USART0_SERIALDRIVER=y +CONFIG_USART1_SERIALDRIVER=y +# CONFIG_USART2_SERIALDRIVER is not set +# CONFIG_USART3_SERIALDRIVER is not set +# CONFIG_USART4_SERIALDRIVER is not set +# CONFIG_USART5_SERIALDRIVER is not set +# CONFIG_USART6_SERIALDRIVER is not set +# CONFIG_USART7_SERIALDRIVER is not set +# CONFIG_USART8_SERIALDRIVER is not set +# CONFIG_OTHER_UART_SERIALDRIVER is not set +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_SERIAL_DMA is not set +CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y +# CONFIG_USART0_SERIAL_CONSOLE is not set +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_OTHER_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART0 Configuration +# +CONFIG_USART0_RXBUFSIZE=256 +CONFIG_USART0_TXBUFSIZE=256 +CONFIG_USART0_BAUD=115200 +CONFIG_USART0_BITS=8 +CONFIG_USART0_PARITY=0 +CONFIG_USART0_2STOP=0 +# CONFIG_USART0_IFLOWCONTROL is not set +# CONFIG_USART0_OFLOWCONTROL is not set +# CONFIG_USART0_DMA is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set +# CONFIG_USART1_DMA is not set +# CONFIG_PSEUDOTERM is not set +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +CONFIG_USBDEV_DUALSPEED=y +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_ARCH_USBDEV_STALLQUEUE is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=768 +CONFIG_CDCACM_RXBUFSIZE=513 +CONFIG_CDCACM_TXBUFSIZE=769 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x006e +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 SAME70-XPLD V1.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_USBMISC is not set +# CONFIG_HAVE_USBTRACE is not set +# CONFIG_DRIVERS_WIRELESS is not set +# CONFIG_DRIVERS_CONTACTLESS is not set + +# +# System Logging +# +# CONFIG_ARCH_SYSLOG is not set +CONFIG_SYSLOG_WRITE=y +# CONFIG_RAMLOG is not set +# CONFIG_SYSLOG_BUFFER is not set +# CONFIG_SYSLOG_INTBUFFER is not set +# CONFIG_SYSLOG_TIMESTAMP is not set +CONFIG_SYSLOG_SERIAL_CONSOLE=y +# CONFIG_SYSLOG_CHAR is not set +CONFIG_SYSLOG_CONSOLE=y +# CONFIG_SYSLOG_NONE is not set +# CONFIG_SYSLOG_FILE is not set +# CONFIG_SYSLOG_CHARDEV is not set + +# +# Networking Support +# +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set +# CONFIG_NET is not set + +# +# Crypto API +# +# CONFIG_CRYPTO is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_AUTOMOUNTER is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_PSEUDOFS_SOFTLINKS is not set +CONFIG_FS_READABLE=y +CONFIG_FS_WRITABLE=y +# CONFIG_FS_AIO is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +CONFIG_FS_MQUEUE_MPATH="/var/mqueue" +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_FORCE_INDIRECT is not set +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_DIRECT_RETRY=y +# CONFIG_FS_NXFFS is not set +CONFIG_FS_ROMFS=y +# CONFIG_FS_TMPFS is not set +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_REGISTER=y + +# +# Exclude individual procfs entries +# +# CONFIG_FS_PROCFS_EXCLUDE_PROCESS is not set +# CONFIG_FS_PROCFS_EXCLUDE_UPTIME is not set +# CONFIG_FS_PROCFS_EXCLUDE_MOUNTS is not set +# CONFIG_FS_PROCFS_EXCLUDE_MTD is not set +# CONFIG_FS_PROCFS_EXCLUDE_PARTITIONS is not set +# CONFIG_FS_UNIONFS is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=1 +# CONFIG_ARCH_HAVE_HEAP2 is not set +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +CONFIG_GRAN_INTR=y + +# +# Common I/O Buffer Support +# +# CONFIG_MM_IOB is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Wireless Support +# +# CONFIG_WIRELESS is not set + +# +# Binary Loader +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# + +# +# Standard C I/O +# +# CONFIG_STDIO_DISABLE_BUFFERING is not set +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +# CONFIG_LIBC_SCANSET is not set +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y + +# +# Architecture-Specific Support +# +CONFIG_ARCH_LOWPUTC=y +# CONFIG_ARCH_ROMGETC is not set +CONFIG_LIBC_ARCH_MEMCPY=y +# CONFIG_LIBC_ARCH_MEMCMP is not set +# CONFIG_LIBC_ARCH_MEMMOVE is not set +# CONFIG_LIBC_ARCH_MEMSET is not set +# CONFIG_LIBC_ARCH_STRCHR is not set +# CONFIG_LIBC_ARCH_STRCMP is not set +# CONFIG_LIBC_ARCH_STRCPY is not set +# CONFIG_LIBC_ARCH_STRNCPY is not set +# CONFIG_LIBC_ARCH_STRLEN is not set +# CONFIG_LIBC_ARCH_STRNLEN is not set +# CONFIG_LIBC_ARCH_ELF is not set +CONFIG_ARMV7M_MEMCPY=y + +# +# stdlib Options +# +CONFIG_LIB_RAND_ORDER=1 +CONFIG_LIB_HOMEDIR="/" +CONFIG_LIBC_TMPDIR="/tmp" +CONFIG_LIBC_MAX_TMPFILE=32 + +# +# Program Execution Options +# +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 + +# +# errno Decode Support +# +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set + +# +# memcpy/memset Options +# +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_LIBC_DLLFCN is not set +# CONFIG_LIBC_MODLIB is not set +# CONFIG_LIBC_WCHAR is not set +# CONFIG_LIBC_LOCALE is not set + +# +# Time/Time Zone Support +# +# CONFIG_LIBC_LOCALTIME is not set +CONFIG_TIME_EXTENDED=y +CONFIG_ARCH_HAVE_TLS=y + +# +# Thread Local Storage (TLS) +# +# CONFIG_TLS is not set + +# +# Network-Related Options +# +# CONFIG_LIBC_IPv4_ADDRCONV is not set +# CONFIG_LIBC_IPv6_ADDRCONV is not set +# CONFIG_LIBC_NETDB is not set + +# +# NETDB Support +# +# CONFIG_NETDB_HOSTFILE is not set +# CONFIG_LIBC_IOCTL_VARIADIC is not set +CONFIG_LIB_SENDFILE_BUFSIZE=512 + +# +# Non-standard Library Support +# +# CONFIG_LIB_CRC64_FAST is not set +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set +# CONFIG_LIB_HEX2BIN is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +# CONFIG_CXX_NEWLONG is not set + +# +# LLVM C++ Library (libcxx) +# +# CONFIG_LIBCXX is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# CAN Utilities +# + +# +# Examples +# +# CONFIG_EXAMPLES_CCTYPE is not set +# CONFIG_EXAMPLES_CHAT is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FSTEST is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_MEDIA is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MOUNT_BLOCKDEVICE is not set +CONFIG_EXAMPLES_MOUNT_NSECTORS=2048 +CONFIG_EXAMPLES_MOUNT_SECTORSIZE=512 +CONFIG_EXAMPLES_MOUNT_RAMDEVNO=0 +# CONFIG_EXAMPLES_MTDPART is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NSH_CXXINITIALIZE is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTERM is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PCA9635 is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_PPPD is not set +# CONFIG_EXAMPLES_RFID_READUID is not set +# CONFIG_EXAMPLES_RGBLED is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMP is not set +# CONFIG_EXAMPLES_STAT is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UNIONFS is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_WATCHDOG is not set +# CONFIG_EXAMPLES_WEBSERVER is not set +# CONFIG_EXAMPLES_XBC_TEST is not set + +# +# File System Utilities +# +# CONFIG_FSUTILS_FLASH_ERASEALL is not set +# CONFIG_FSUTILS_INIFILE is not set +# CONFIG_FSUTILS_PASSWD is not set + +# +# GPS Utilities +# +# CONFIG_GPSUTILS_MINMEA_LIB is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_BAS is not set +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_MICROPYTHON is not set +# CONFIG_INTERPRETERS_MINIBASIC is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# Network Utilities +# +# CONFIG_NETUTILS_CHAT is not set +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_ESP8266 is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_THTTPD is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +# CONFIG_NSH_MOTD is not set + +# +# Command Line Configuration +# +CONFIG_NSH_READLINE=y +# CONFIG_NSH_CLE is not set +CONFIG_NSH_LINELEN=128 +# CONFIG_NSH_DISABLE_SEMICOLON is not set +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +CONFIG_NSH_DISABLE_ADDROUTE=y +# CONFIG_NSH_DISABLE_BASENAME is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_CMP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_DF is not set +CONFIG_NSH_DISABLE_DELROUTE=y +# CONFIG_NSH_DISABLE_DIRNAME is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +CONFIG_NSH_DISABLE_HEXDUMP=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +# CONFIG_NSH_DISABLE_KILL is not set +CONFIG_NSH_DISABLE_LOSETUP=y +CONFIG_NSH_DISABLE_LOSMART=y +# CONFIG_NSH_DISABLE_LS is not set +CONFIG_NSH_DISABLE_MB=y +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +CONFIG_NSH_DISABLE_MKFIFO=y +CONFIG_NSH_DISABLE_MKRD=y +CONFIG_NSH_DISABLE_MH=y +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_MW is not set +CONFIG_NSH_DISABLE_PRINTF=y +# CONFIG_NSH_DISABLE_PS is not set +CONFIG_NSH_DISABLE_PSSTACKUSAGE=y +CONFIG_NSH_DISABLE_PUT=y +# CONFIG_NSH_DISABLE_PWD is not set +CONFIG_NSH_DISABLE_REBOOT=y +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +CONFIG_NSH_DISABLE_SHUTDOWN=y +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNAME is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_NSH_DISABLE_WGET=y +CONFIG_NSH_DISABLE_XD=y +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +# CONFIG_NSH_CMDOPT_DD_STATS is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_PROC_MOUNTPOINT="/proc" +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y + +# +# Scripting Support +# +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +# CONFIG_NSH_DEFAULTROMFS is not set +CONFIG_NSH_ARCHROMFS=y +# CONFIG_NSH_CUSTOMROMFS is not set +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" + +# +# Console Configuration +# +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set +# CONFIG_NSH_ALTCONDEV is not set +CONFIG_NSH_ARCHINIT=y +# CONFIG_NSH_LOGIN is not set +# CONFIG_NSH_CONSOLE_LOGIN is not set + +# +# NxWidgets/NxWM +# + +# +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set +CONFIG_HAVE_CXXINITIALIZE=y + +# +# System Libraries and NSH Add-Ons +# +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CDCACM_DEVMINOR=0 +# CONFIG_SYSTEM_CLE is not set +# CONFIG_SYSTEM_CUTERM is not set +# CONFIG_SYSTEM_FLASH_ERASEALL is not set +# CONFIG_SYSTEM_FREE is not set +# CONFIG_SYSTEM_HEX2BIN is not set +# CONFIG_SYSTEM_HEXED is not set +# CONFIG_SYSTEM_I2CTOOL is not set +# CONFIG_SYSTEM_INSTALL is not set +# CONFIG_SYSTEM_RAMTEST is not set +CONFIG_READLINE_HAVE_EXTMATCH=y +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y +# CONFIG_READLINE_TABCOMPLETION is not set +# CONFIG_READLINE_CMD_HISTORY is not set +# CONFIG_SYSTEM_STACKMONITOR is not set +# CONFIG_SYSTEM_SUDOKU is not set +# CONFIG_SYSTEM_SYSTEM is not set +# CONFIG_SYSTEM_TEE is not set +# CONFIG_SYSTEM_UBLOXMODEM is not set +# CONFIG_SYSTEM_VI is not set +# CONFIG_SYSTEM_ZMODEM is not set + +# +# Wireless Libraries and NSH Add-Ons +# + +# +# IEEE 802.15.4 applications +# +# CONFIG_IEEE802154_LIBMAC is not set +# CONFIG_IEEE802154_LIBUTILS is not set +# CONFIG_IEEE802154_I8SAK is not set diff --git a/nuttx-configs/px4-same70xplained-v1/nsh/setenv.sh b/nuttx-configs/px4-same70xplained-v1/nsh/setenv.sh new file mode 100755 index 0000000000..45dc13b84f --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/nsh/setenv.sh @@ -0,0 +1,80 @@ +#!/bin/bash +# configs/samv7-xult/nsh/Make.defs +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the Atmel GCC +# toolchain under Windows. You will also have to edit this if you install +# this toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Atmel/Atmel Toolchain/ARM GCC/Native/4.7.3.99/arm-gnu-toolchain/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery_CodeBench_Lite_for_ARM_EABI/bin" +# export TOOLCHAIN_BIN="/cygdrive/c/Users/MyName/MentorGraphics/Sourcery_CodeBench_Lite_for_ARM_EABI/bin" + +# This is the location where I installed the ARM "GNU Tools for ARM Embedded Processors" +# You can this free toolchain here https://launchpad.net/gcc-arm-embedded +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/GNU Tools ARM Embedded/4.9 2015q2/bin" + +# This is the path to the location where I installed the devkitARM toolchain +# You can get this free toolchain from http://devkitpro.org/ or http://sourceforge.net/projects/devkitpro/ +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/devkitARM/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +# export TOOLCHAIN_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" + +# The same70-xplained/tools directory +export TOOL_DIR="${WD}/configs/same70-xplained/tools" + +# Add the path to the toolchain and tools directory to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:${TOOL_DIR}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4-same70xplained-v1/scripts/flash-dtcm.ld b/nuttx-configs/px4-same70xplained-v1/scripts/flash-dtcm.ld new file mode 100644 index 0000000000..28f618fe41 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/flash-dtcm.ld @@ -0,0 +1,122 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/flash-dtcm.ld + * + * 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 SAME70Q21 has 2048Kb of FLASH beginning at address 0x0040:0000 and + * 384Kb of SRAM beginining at 0x2040:0000 + * + * 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 0x0400:0000 address range (Assuming that ITCM is not enable). + * + * NOTE: that the DTCM address of 0x2000:0000 is used for SRAM. If DTCM is + * disabled, then the accesses will actually occur on the AHB bus. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x00400000, LENGTH = 2048K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 384K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _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/nuttx-configs/px4-same70xplained-v1/scripts/flash-sram.ld b/nuttx-configs/px4-same70xplained-v1/scripts/flash-sram.ld new file mode 100644 index 0000000000..e1dff4a10f --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/flash-sram.ld @@ -0,0 +1,119 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/flash-sram.ld + * + * 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 SAME70Q21 has 2048Kb of FLASH beginning at address 0x0040:0000 and + * 384Kb of SRAM beginining at 0x2040:0000 + * + * 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 0x0400:0000 address range (Assuming that ITCM is not enable). + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x00400000, LENGTH = 2048K + sram (rwx) : ORIGIN = 0x20400000, LENGTH = 384K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _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/nuttx-configs/px4-same70xplained-v1/scripts/gnu-elf.ld b/nuttx-configs/px4-same70xplained-v1/scripts/gnu-elf.ld new file mode 100644 index 0000000000..d811db5a69 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/gnu-elf.ld @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/gnu-elf.ld + * + * 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. + * + ****************************************************************************/ + +SECTIONS +{ + .text 0x00000000 : + { + _stext = . ; + *(.text) + *(.text.*) + *(.gnu.warning) + *(.stub) + *(.glue_7) + *(.glue_7t) + *(.jcr) + + /* C++ support: The .init and .fini sections contain specific logic + * to manage static constructors and destructors. + */ + + *(.gnu.linkonce.t.*) + *(.init) /* Old ABI */ + *(.fini) /* Old ABI */ + _etext = . ; + } + + .rodata : + { + _srodata = . ; + *(.rodata) + *(.rodata1) + *(.rodata.*) + *(.gnu.linkonce.r*) + _erodata = . ; + } + + .data : + { + _sdata = . ; + *(.data) + *(.data1) + *(.data.*) + *(.gnu.linkonce.d*) + _edata = . ; + } + + /* C++ support. For each global and static local C++ object, + * GCC creates a small subroutine to construct the object. Pointers + * to these routines (not the routines themselves) are stored as + * simple, linear arrays in the .ctors section of the object file. + * Similarly, pointers to global/static destructor routines are + * stored in .dtors. + */ + + .ctors : + { + _sctors = . ; + *(.ctors) /* Old ABI: Unallocated */ + *(.init_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .dtors : + { + _sdtors = . ; + *(.dtors) /* Old ABI: Unallocated */ + *(.fini_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .bss : + { + _sbss = . ; + *(.bss) + *(.bss.*) + *(.sbss) + *(.sbss.*) + *(.gnu.linkonce.b*) + *(COMMON) + _ebss = . ; + } + + /* 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/nuttx-configs/px4-same70xplained-v1/scripts/kernel-space.ld b/nuttx-configs/px4-same70xplained-v1/scripts/kernel-space.ld new file mode 100644 index 0000000000..8864c96b9e --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/kernel-space.ld @@ -0,0 +1,111 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/kernel-space.ld + * + * 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. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > kflash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > kflash + + .ARM.extab : { + *(.ARM.extab*) + } > kflash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > kflash + + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > ksram AT > kflash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > ksram + + /* 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/nuttx-configs/px4-same70xplained-v1/scripts/ld.script b/nuttx-configs/px4-same70xplained-v1/scripts/ld.script new file mode 100644 index 0000000000..bd3fb4b4d3 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/ld.script @@ -0,0 +1,144 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/flash-sram.ld + * + * 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 SAME70Q21 has 2048Kb of FLASH beginning at address 0x0040:0000 and + * 384Kb of SRAM beginining at 0x2040:0000 + * + * 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 0x0400:0000 address range (Assuming that ITCM is not enable). + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x00400000, LENGTH = 2048K + sram (rwx) : ORIGIN = 0x20400000, LENGTH = 384K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.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(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _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/nuttx-configs/px4-same70xplained-v1/scripts/memory.ld b/nuttx-configs/px4-same70xplained-v1/scripts/memory.ld new file mode 100644 index 0000000000..65673b3d84 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/memory.ld @@ -0,0 +1,84 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/memory.ld + * + * 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 SAME70Q21 has 2048Kb of FLASH beginning at address 0x0040:0000 and + * 384Kb of SRAM beginining at 0x2040:0000 + * + * 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 0x0400:0000 address range. + * + * NOTE: that the DTCM address of 0x2000:0000 is used for SRAM. If DTCM is + * disabled, then the accesses will actually occur on the AHB bus. + * + * The user space partition will be spanned with a single region of size + * 2**n bytes. The alignment of the user-space region must be the same. + * As a consequence, as the user-space increases in size, the alignment + * requirement also increases. The sizes below give the largest possible + * user address spaces (but leave far too much for the OS). + * + * The solution to this wasted memory is to (1) use more than one region to + * span the user spaces, or (2) poke holes in a larger region to trim it + * to fit better. + * + * A detailed memory map for the 384KB SRAM region is as follows: + * + * 0x20000 0000: Kernel .data region. Typical size: 0.1KB + * ------- ---- Kernel .bss region. Typical size: 1.8KB + * 0x20000 0800: Kernel IDLE thread stack (approximate). Size is + * determined by CONFIG_IDLETHREAD_STACKSIZE and + * adjustments for alignment. Typical is 1KB. + * ------- ---- Padded to 4KB + * 0x20002 0000: User .data region. Size is variable. + * ------- ---- User .bss region Size is variable. + * 0x20000 2000: Beginning of kernel heap. Size determined by + * CONFIG_MM_KERNEL_HEAPSIZE. + * ------- ---- Beginning of user heap. Can vary with other settings. + * 0x20004 0000: End+1 of mappable internal SRAM + */ + +MEMORY +{ + /* 2048KiB of internal FLASH */ + + kflash (rx) : ORIGIN = 0x00400000, LENGTH = 1M + uflash (rx) : ORIGIN = 0x00500000, LENGTH = 1M + + /* 384Kb of internal SRAM */ + + ksram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + usram (rwx) : ORIGIN = 0x20020000, LENGTH = 128K + xsram (rwx) : ORIGIN = 0x20040000, LENGTH = 128K +} diff --git a/nuttx-configs/px4-same70xplained-v1/scripts/user-space.ld b/nuttx-configs/px4-same70xplained-v1/scripts/user-space.ld new file mode 100644 index 0000000000..bdb149ca83 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/scripts/user-space.ld @@ -0,0 +1,126 @@ +/**************************************************************************** + * configs/same70-xplained/scripts/user-space.ld + * + * 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. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ + +/* Make sure that the critical memory management functions are in user-space. + * the user heap memory manager will reside in user-space but be usable both + * by kernel- and user-space code + */ + +EXTERN(umm_initialize) +EXTERN(umm_addregion) +EXTERN(umm_trysemaphore) +EXTERN(umm_givesemaphore) + +EXTERN(malloc) +EXTERN(realloc) +EXTERN(zalloc) +EXTERN(free) + +OUTPUT_ARCH(arm) +SECTIONS +{ + .userspace : { + *(.userspace) + } > uflash + + .text : { + _stext = ABSOLUTE(.); + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > uflash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > uflash + + .ARM.extab : { + *(.ARM.extab*) + } > uflash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > uflash + + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > usram AT > uflash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > usram + + /* 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/nuttx-configs/px4-same70xplained-v1/src/Makefile b/nuttx-configs/px4-same70xplained-v1/src/Makefile new file mode 100644 index 0000000000..35ad52a2d3 --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/src/Makefile @@ -0,0 +1,83 @@ +############################################################################ +# configs/px4fmu-v2_upstream/src/Makefile +# +# Copyright (C) 2011 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx-configs/px4-same70xplained-v1/src/empty.c b/nuttx-configs/px4-same70xplained-v1/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/px4-same70xplained-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/common/samv7/board_identity.c b/src/drivers/boards/common/samv7/board_identity.c new file mode 100644 index 0000000000..12ea475610 --- /dev/null +++ b/src/drivers/boards/common/samv7/board_identity.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file board_identity.c + * Implementation of SamV7 based Board identity API + */ + +#include +#include +#include +#include +#include +#include + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +const uint32_t DUMMY_SIM_UIDH[4] = {0x12345678, 0x12345678, 0x12345678, 0x12345678 }; + +void board_get_uuid(uuid_byte_t uuid_bytes) +{ + uint32_t *chip_uuid = (uint32_t *) DUMMY_SIM_UIDH; + uint32_t *uuid_words = (uint32_t *) uuid_bytes; + + for (int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + *uuid_words++ = SWAP_UINT32(chip_uuid[i]); + } +} + +void board_get_uuid32(uuid_uint32_t uuid_words) +{ + board_get_uuid(*(uuid_byte_t *) uuid_words); +} + +int board_get_uuid32_formated(char *format_buffer, int size, + const char *format, + const char *seperator) +{ + uuid_uint32_t uuid; + board_get_uuid32(uuid); + + int offset = 0; + int sep_size = seperator ? strlen(seperator) : 0; + + for (int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]); + + if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) { + strcat(&format_buffer[offset], seperator); + offset += sep_size; + } + } + + return 0; +} + +int board_get_mfguid(mfguid_t mfgid) +{ + board_get_uuid(* (uuid_byte_t *) mfgid); + return PX4_CPU_MFGUID_BYTE_LENGTH; +} + +int board_get_mfguid_formated(char *format_buffer, int size) +{ + mfguid_t mfguid; + + board_get_mfguid(mfguid); + int offset = 0; + + for (unsigned int i = 0; i < PX4_CPU_MFGUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", mfguid[i]); + } + + return offset; +} diff --git a/src/drivers/boards/common/samv7/board_mcu_version.c b/src/drivers/boards/common/samv7/board_mcu_version.c new file mode 100644 index 0000000000..31a4f9c82d --- /dev/null +++ b/src/drivers/boards/common/samv7/board_mcu_version.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file board_mcu_version.c + * Implementation of SamV7 based SoC version API + */ + +#include +#include + +/* Define any issues with the Silicon as lines separated by \n + * omitting the last \n + */ +#define ERRATA "This code is not finished yet!" + + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + *rev = '?'; + *revstr = "SAM V??"; + + if (errata) { + *errata = ERRATA; + } + + return 0; +} diff --git a/src/drivers/boards/common/samv7/board_reset.c b/src/drivers/boards/common/samv7/board_reset.c new file mode 100644 index 0000000000..566f6ea5f6 --- /dev/null +++ b/src/drivers/boards/common/samv7/board_reset.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file board_reset.c + * Implementation of SamV7 based Board RESET API + */ + +#include +#include +#include + + +int board_set_bootload_mode(board_reset_e mode) +{ + uint32_t regvalue = 0; + + switch (mode) { + case board_reset_normal: + case board_reset_extended: + break; + + case board_reset_enter_bootloader: + regvalue = 0xb007b007; + break; + + default: + return -EINVAL; + } + +// todo: Add a way to enter bootloader + UNUSED(regvalue); + return OK; +} + + +void board_system_reset(int status) +{ +#if defined(BOARD_HAS_ON_RESET) + board_on_reset(status); +#endif + board_reset(status); + + while (1); +} diff --git a/src/drivers/boards/px4-same70xplained-v1/CMakeLists.txt b/src/drivers/boards/px4-same70xplained-v1/CMakeLists.txt new file mode 100644 index 0000000000..1f5ae12417 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# 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 drivers__boards__px4-same70xplained-v1 + COMPILE_FLAGS + -Os + SRCS + ../common/samv7/board_reset.c + ../common/samv7/board_identity.c + ../common/samv7/board_mcu_version.c + ../common/board_crashdump.c + ../common/board_dma_alloc.c + px4same70xplained_can.c + px4same70xplained_init.c + px4same70xplained_timer_config.c + px4same70xplained_spi.c + px4same70xplained_usb.c + px4same70xplained_led.c + px4same70xplained_sdram.c + DEPENDS + platforms__common + ) diff --git a/src/drivers/boards/px4-same70xplained-v1/board_config.h b/src/drivers/boards/px4-same70xplained-v1/board_config.h new file mode 100644 index 0000000000..7e7622b18d --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/board_config.h @@ -0,0 +1,562 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2017 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 + * + * PX4SAME70_XPLAINEDV1 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4_SAME70XPLAINED_V1 GPIO Pin Definitions *************************************************/ + +/* Ethernet MAC. + * + * KSZ8081RNACA Connections + * ------------------------ + * + * ------ --------- --------- + * SAME70 SAME70 Ethernet + * Pin Function Functio + * ------ --------- --------- + * PD0 GTXCK REF_CLK + * PD1 GTXEN TXEN + * PD2 GTX0 TXD0 + * PD3 GTX1 TXD1 + * PD4 GRXDV CRS_DV + * PD5 GRX0 RXD0 + * PD6 GRX1 RXD1 + * PD7 GRXER RXER + * PD8 GMDC MDC + * PD9 GMDIO MDIO + * PA14 GPIO INTERRUPT + * PC10 GPIO RESET + * ------ --------- --------- + */ + +#define GPIO_EMAC0_INT (GPIO_INPUT | GPIO_CFG_PULLUP | GPIO_CFG_DEGLITCH | \ + GPIO_INT_FALLING | GPIO_PORT_PIOA | GPIO_PIN14) +#define GPIO_EMAC0_RESET (GPIO_OUTPUT | GPIO_CFG_PULLUP | GPIO_OUTPUT_SET | \ + GPIO_PORT_PIOC | GPIO_PIN10) + +#define IRQ_EMAC0_INT SAM_IRQ_PA14 + +/* LEDs + * + * A single LED is available driven by PC8. + */ + +#define GPIO_LED1 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | \ + GPIO_PORT_PIOC | GPIO_PIN8) + +/* Buttons + * + * SAM E70 Xplained contains two mechanical buttons. One button is the RESET + * button connected to the SAM E70 reset line and the other, PA11, is a generic + * user configurable button. When a button is pressed it will drive the I/O + * line to GND. + * + * NOTE: There are no pull-up resistors connected to the generic user buttons + * so it is necessary to enable the internal pull-up in the SAM E70 to use the + * button. + */ + +#define GPIO_SW0 (GPIO_INPUT | GPIO_CFG_PULLUP | GPIO_CFG_DEGLITCH | \ + GPIO_INT_BOTHEDGES | GPIO_PORT_PIOA | GPIO_PIN11) +#define IRQ_SW0 SAM_IRQ_PA11 + +/* HSMCI SD Card Detect + * + * The SAM E70 Xplained has one standard SD card connector that is connected + * to the High Speed Multimedia Card Interface (HSMCI) of the SAM E70. SD + * card connector: + * + * ------ ----------------- --------------------- + * SAME70 SAME70 Shared functionality + * Pin Function + * ------ ----------------- --------------------- + * PA30 MCDA0 (DAT0) + * PA31 MCDA1 (DAT1) + * PA26 MCDA2 (DAT2) + * PA27 MCDA3 (DAT3) Camera + * PA25 MCCK (CLK) Shield + * PA28 MCCDA (CMD) + * PC16 Card Detect (C/D) Shield + * ------ ----------------- --------------------- + */ + +#define GPIO_MCI0_CD (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_INT_BOTHEDGES | GPIO_PORT_PIOC | GPIO_PIN16) +#define IRQ_MCI0_CD SAM_IRQ_PC16 + +/* USB Host + * + * The SAM E70 Xplained has a Micro-USB connector for use with the SAM E70 USB + * module labeled as TARGET USB on the kit. In USB host mode VBUS voltage is + * provided by the kit and has to be enabled by setting the "VBUS Host Enable" + * pin (PC16) low. + */ + +#define GPIO_VBUSON (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | \ + GPIO_PORT_PIOC | GPIO_PIN16) + + + +/* External interrupts */ +//todo:DBS fix all these +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_INT_RISING | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_INT_RISING | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_INT_RISING | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_INT_RISING | GPIO_PORT_PIOD | GPIO_PIN30) + +/* Data ready pins off */ +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN30) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN30) + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN22) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN20) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN21) + +/* SPI1 chip selects off */ +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOC | GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOC | GPIO_PIN17 ) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN11) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOD | GPIO_PIN27) + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN17 ) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOD | GPIO_PIN11) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOD | GPIO_PIN27) + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 + + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +/* No Onboard Sensors #define PX4_I2C_BUS_ONBOARD 0 */ +#define PX4_I2C_BUS_LED 1 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_LIS3MDL 0x1e + +/* Define the follwoing to output the clock on J500-1 */ +//#define GPIO_PCK1 (GPIO_PERIPHB | GPIO_CFG_DEFAULT | GPIO_PORT_PIOA | GPIO_PIN17) + + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* r GPIOs + * + * GPIO0-3 are the PWM servo outputs. + */ + + +/* User GPIOs + * + * GPIO0-3 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOC | GPIO_PIN19) +#define GPIO_GPIO1_INPUT (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOC | GPIO_PIN26) +#define GPIO_GPIO2_INPUT (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOA | GPIO_PIN2) +#define GPIO_GPIO3_INPUT (GPIO_INPUT | GPIO_CFG_DEFAULT | GPIO_CFG_DEGLITCH | \ + GPIO_CFG_PULLDOWN | GPIO_PORT_PIOA | GPIO_PIN24) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN19) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOC | GPIO_PIN26) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOA | GPIO_PIN2) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | GPIO_PORT_PIOA | GPIO_PIN24) + + +/* Tone alarm output */ +#define TONE_ALARM_CHANNEL 5 /* channel TC 1 Chan 5 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_CLEAR | GPIO_PORT_PIOC | GPIO_PIN30) +#define GPIO_TONE_ALARM GPIO_TC5_TIOB + +#if 0 + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 +#endif +#define DIRECT_PWM_OUTPUT_CHANNELS 1 +#define DIRECT_INPUT_TIMER_CHANNELS 1 + +/* High-resolution timer */ +/* sam timers are usually addressed by channel number 0-11 + * Timer 0 has Channel 0,1,2, Timer 1 has channels 3, 4, 5, + * Timer 2 has Channel 6,7,8, Timer 3 has channels 9, 10, 11, + */ +#define HRT_TIMER_CHANNEL 2 /* use channel 2 */ + +#if 0 +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 +#endif + +#define BOARD_NAME "PX4_SAME70XPLAINED_V1" + +/* + * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +/* +#define BOARD_ADC_USB_CONNECTED board_read_VBUS_state() +#define BOARD_ADC_BRICK_VALID (1) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) +*/ + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, } + +/* + * GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/************************************************************************************ + * Name: sam_sdram_config + * + * Description: + * Configures the on-board SDRAM. SAME70 Xplained features one external + * IS42S16100E-7BLI, 512Kx16x2, 10ns, SDRAM. SDRAM0 is connected to chip select + * NCS1. + * + * Input Parameters: + * None + * + * Assumptions: + * The DDR memory regions is configured as strongly ordered memory. When we + * complete initialization of SDRAM and it is ready for use, we will make DRAM + * into normal memory. + * + ************************************************************************************/ + +#ifdef CONFIG_SAMV7_SDRAMC +void sam_sdram_config(void); +#else +# define sam_sdram_config(t) +#endif + +/************************************************************************************ + * Name: sam_bringup + * + * Description: + * Bring up board features + * + ************************************************************************************/ + +#if defined(CONFIG_LIB_BOARDCTL) || defined(CONFIG_BOARD_INITIALIZE) +int sam_bringup(void); +#endif + +/************************************************************************************ + * Name: sam_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the SAME70-XPLD board. + * + ************************************************************************************/ + +#ifdef CONFIG_SAMV7_SPI +#define board_spi_reset(ms) +void board_spi_initialize(void); +#endif + +#define board_peripheral_reset(ms) + +/************************************************************************************ + * Name: sam_hsmci_initialize + * + * Description: + * Initialize HSMCI support + * + ************************************************************************************/ + +#ifdef HAVE_HSMCI +int sam_hsmci_initialize(int slot, int minor); +#else +# define sam_hsmci_initialize(s,m) (-ENOSYS) +#endif + +/************************************************************************************ + * Name: board_usb_initialize + * + * Description: + * Called from stm32_boardinitialize very early in initialization to setup USB- + * related GPIO pins for the SAME70-XPLD board. + * + ************************************************************************************/ + +void board_usb_initialize(void); + +/************************************************************************************ + * Name: sam_netinitialize + * + * Description: + * Configure board resources to support networking. + * + ************************************************************************************/ + +#ifdef HAVE_NETWORK +void sam_netinitialize(void); +#endif + +/************************************************************************************ + * Name: sam_emac0_setmac + * + * Description: + * Read the Ethernet MAC address from the AT24 FLASH and configure the Ethernet + * driver with that address. + * + ************************************************************************************/ + +#ifdef HAVE_MACADDR +int sam_emac0_setmac(void); +#endif + +/************************************************************************************ + * Name: sam_cardinserted + * + * Description: + * Check if a card is inserted into the selected HSMCI slot + * + ************************************************************************************/ + +#ifdef HAVE_HSMCI +bool sam_cardinserted(int slotno); +#else +# define sam_cardinserted(slotno) (false) +#endif + +/************************************************************************************ + * Name: sam_writeprotected + * + * Description: + * Check if the card in the MMCSD slot is write protected + * + ************************************************************************************/ + +#ifdef HAVE_HSMCI +bool sam_writeprotected(int slotno); +#endif + +/************************************************************************************ + * Name: sam_automount_initialize + * + * Description: + * Configure auto-mounters for each enable and so configured HSMCI + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +void sam_automount_initialize(void); +#endif + +/************************************************************************************ + * Name: sam_automount_event + * + * Description: + * The HSMCI card detection logic has detected an insertion or removal event. It + * has already scheduled the MMC/SD block driver operations. Now we need to + * schedule the auto-mount event which will occur with a substantial delay to make + * sure that everything has settle down. + * + * Input Parameters: + * slotno - Identifies the HSMCI0 slot: HSMCI0 or HSMCI1_SLOTNO. There is a + * terminology problem here: Each HSMCI supports two slots, slot A and slot B. + * Only slot A is used. So this is not a really a slot, but an HSCMI peripheral + * number. + * inserted - True if the card is inserted in the slot. False otherwise. + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled. + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +void sam_automount_event(int slotno, bool inserted); +#endif + +/************************************************************************************ + * Name: sam_writeprotected + * + * Description: + * Check if the card in the MMCSD slot is write protected + * + ************************************************************************************/ + +#ifdef HAVE_HSMCI +bool sam_writeprotected(int slotno); +#else +# define sam_writeprotected(slotno) (false) +#endif + +/************************************************************************************ + * Name: sam_at24config + * + * Description: + * Create an AT24xx-based MTD configuration device for storage device configuration + * information. + * + ************************************************************************************/ + +#ifdef HAVE_MTDCONFIG +int sam_at24config(void); +#endif + +#include "../common/board_common.h" + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4-same70xplained-v1/px4fmu_timer_config.c b/src/drivers/boards/px4-same70xplained-v1/px4fmu_timer_config.c new file mode 100644 index 0000000000..5adc4260db --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4fmu_timer_config.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM1CC, + + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .first_channel_index = 4, + .last_channel_index = 5, + .handler = io_timer_handler1, + .vectorno = STM32_IRQ_TIM4 + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM1_CH4OUT, + .gpio_in = GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + }, + { + .gpio_out = GPIO_TIM1_CH3OUT, + .gpio_in = GPIO_TIM1_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM1_CH2OUT, + .gpio_in = GPIO_TIM1_CH2IN, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM1_CH1OUT, + .gpio_in = GPIO_TIM1_CH1IN, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM4_CH2OUT, + .gpio_in = GPIO_TIM4_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM4_CH3OUT, + .gpio_in = GPIO_TIM4_CH3IN, + .timer_index = 1, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + } +}; diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_can.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_can.c new file mode 100644 index 0000000000..2df592651d --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2017 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 px4same70xplained_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include +#include "board_config.h" + +#include "chip.h" +#include "up_arch.h" + +#include "chip.h" +#include "sam_mcan.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if (defined(CONFIG_SAMV7_MCAN0) || defined(CONFIG_SAMV7_MCAN1)) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_SAMV7_MCAN0 +#endif + +#ifdef CONFIG_SAMV7_MCAN0 +# define CAN_PORT 0 +#else +# define CAN_PORT 1 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All SAM architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int board_can_initialize(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call sam_mcan_initialize() to get an instance of the CAN interface */ + + can = sam_mcan_initialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_init.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_init.c new file mode 100644 index 0000000000..35af6eb516 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_init.c @@ -0,0 +1,374 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 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 px4same70xplained_init.c + * + * PX4_SAME70XPLAINED_V1 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 "platform/cxxinitialize.h" +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) syslog(LOG_NOTICE, __VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message syslog +# else +# define message printf +# endif +#endif + +/* + * 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); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: sam_boardinitialize + * + * Description: + * All SAMV7 architectures must provide the following entry point. This + * entry point is called early in the initialization -- after clocking and + * memory have been configured but before caches have been enabled and + * before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +sam_boardinitialize(void) +{ +#ifdef CONFIG_SCHED_TICKLESS + uint32_t frequency; + uint32_t actual; + + /* If Tickless mode is selected then enabled PCK6 as a possible clock + * source for the timer/counters. The ideal frequency could be: + * + * frequency = 1,000,000 / CONFIG_USEC_PER_TICK + * + * The main crystal is selected as the frequency source. The maximum + * prescaler value is 256 so the minimum frequency is 46,875 Hz which + * corresponds to a period of 21.3 microseconds. A value of + * CONFIG_USEC_PER_TICK=20, or 50KHz, would give an exact solution with + * a divider of 240. + */ + + frequency = USEC_PER_SEC / CONFIG_USEC_PER_TICK; + DEBUGASSERT(frequency >= (BOARD_MAINOSC_FREQUENCY / 256)); + + actual = sam_pck_configure(PCK6, PCKSRC_MAINCK, frequency); + + /* We expect to achieve this frequency exactly */ + + DEBUGASSERT(actual == frequency); + UNUSED(actual); + + /* Enable PCK6 */ + + (void)sam_pck_enable(PCK6, true); +#endif + + /* Lets bring the clock out for a sanity check*/ +#ifdef GPIO_PCK1 + sam_configgpio(GPIO_PCK1); + volatile uint32_t actual = sam_pck_configure(PCK1, PCKSRC_MCK, BOARD_MCK_FREQUENCY / 2); // Out 1/2 Clock + UNUSED(actual); + + (void)sam_pck_enable(PCK1, true); +#endif +#ifdef CONFIG_SAMV7_SDRAMC + /* Configure SDRAM if it has been enabled in the NuttX configuration. + * Here we assume, of course, that we are not running out SDRAM. + */ + + sam_sdram_config(); +#endif + +#ifdef CONFIG_SAMV7_SPI + + /* configure SPI interfaces */ + + board_spi_initialize(); +#endif + +#ifdef CONFIG_ARCH_LEDS + + /* configure LEDs */ + + board_autoled_initialize(); +#endif +} + +/**************************************************************************** + * 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. + * + ****************************************************************************/ + +static struct spi_dev_s *spi0; +#if defined(CONFIG_SAMV7_SPI1_MASTER) +static struct spi_dev_s *spi1; +#endif +static struct sdio_dev_s *sdio; + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + + /* configure ADC pins */ + + /* configure power supply control/sense pins */ + +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + + /* run C++ ctors before we go any further */ + + up_cxxinitialize(); + +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + param_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + message("DMA alloc FAILED"); + } + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + + /* initial LED state */ + drv_led_start(); + led_on(LED_AMBER); + led_off(LED_AMBER); + + /* Configure SPI-based devices */ + + spi0 = sam_spibus_initialize(0); + + if (!spi0) { + message("[boot] FAILED to initialize SPI port 0\n"); + board_autoled_on(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi0, 10000000); + SPI_SETBITS(spi0, 8); + SPI_SETMODE(spi0, SPIDEV_MODE3); + SPI_SELECT(spi0, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi0, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi0, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi0, PX4_SPIDEV_MPU, false); + up_udelay(20); + +#if defined(CONFIG_SAMV7_SPI1_MASTER) + spi1 = sam_spibus_initialize(1); + + /* Default SPI4 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi1, PX4_SPIDEV_EXT1, false); +#endif + +#ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + message("[boot] Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + if (ret != OK) { + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + +#endif + + return OK; +} + +#if defined(CONFIG_BOARDCTL_RESET) +int board_reset(int status) +{ + up_systemreset(); + return 0; +} +#endif + +//FIXME: Stubs -----v +/* g_rtc_enabled is set true after the RTC has successfully initialized */ + +volatile bool g_rtc_enabled = false; + +int up_rtc_getdatetime(FAR struct tm *tp); +int up_rtc_getdatetime(FAR struct tm *tp) +{ + tp->tm_sec = 0; + tp->tm_min = 0; + tp->tm_hour = 0; + tp->tm_mday = 30; + tp->tm_mon = 10; + tp->tm_year = 116; + tp->tm_wday = 1; /* Day of the week (0-6) */ + tp->tm_yday = 0; /* Day of the year (0-365) */ + tp->tm_isdst = 0; /* Non-0 if daylight savings time is in effect */ + return 0; +} + +int up_rtc_initialize(void) +{ + return 0; +} + +int up_rtc_settime(FAR const struct timespec *tp) +{ + return 0; +} +//FIXME: Stubs -----^ diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_led.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_led.c new file mode 100644 index 0000000000..ab16ea5fd3 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_led.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4same70xplained_led.c + * + * PX4_SAME70XPLAINED_V1 LED backend. + */ + +#include + +#include + +#include "sam_gpio.h" +#include +#include +#include "board_config.h" + +/* + * 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 + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + sam_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) { + /* Pull down to switch on */ + sam_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) { + /* Pull up to switch off */ + sam_gpiowrite(GPIO_LED1, true); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) { + if (sam_gpioread(GPIO_LED1)) { + sam_gpiowrite(GPIO_LED1, false); + + } else { + sam_gpiowrite(GPIO_LED1, true); + } + } +} + +#if defined(CONFIG_ARCH_LEDS) +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (led == 1 || led == 3) { + sam_gpiowrite(GPIO_LED1, false); /* Low illuminates */ + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (led == 3) { + sam_gpiowrite(GPIO_LED1, true); /* High extinguishes */ + } +} +#endif diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_sdram.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_sdram.c new file mode 100644 index 0000000000..c1f6c1f250 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_sdram.c @@ -0,0 +1,313 @@ +/**************************************************************************** + * configs/same70-xplained/src/sam_sdram.c + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Most of this file derives from Atmel sample code for the SAME70-XPLD + * board. That sample code has licensing that is compatible with the NuttX + * modified BSD license: + * + * Copyright (c) 2012, Atmel Corporation + * 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 NuttX nor Atmel 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include + +#include "up_arch.h" + +#include "sam_periphclks.h" +#include "chip/sam_memorymap.h" +#include "chip/sam_pinmap.h" +#include "chip/sam_pmc.h" +#include "chip/sam_matrix.h" +#include "chip/sam_sdramc.h" + +#include "board_config.h" + +#ifdef CONFIG_SAMV7_SDRAMC + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define SDRAM_BA0 (1 << 20) +#define SDRAM_BA1 (1 << 21) + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: sam_sdram_config + * + * Description: + * Configures the on-board SDRAM. SAME70 Xplained features one external + * IS42S16100E-7BLI, 512Kx16x2, 10ns, SDRAM. SDRAM0 is connected to chip + * select NCS1. + * + * Input Parameters: + * None + * + * Assumptions: + * This test runs early in initialization before I- and D-caches are + * enabled. + * + * NOTE: Since the delay loop is calibrate with caches in enabled, the + * calls to up_udelay() are wrong ty orders of magnitude. + * + ****************************************************************************/ + +void sam_sdram_config(void) +{ + volatile uint8_t *psdram = (uint8_t *)SAM_SDRAMCS_BASE; + uint32_t regval; + int i; + + /* Configure SDRAM pins */ + + sam_configgpio(GPIO_SMC_D0); + sam_configgpio(GPIO_SMC_D1); + sam_configgpio(GPIO_SMC_D2); + sam_configgpio(GPIO_SMC_D3); + sam_configgpio(GPIO_SMC_D4); + sam_configgpio(GPIO_SMC_D5); + sam_configgpio(GPIO_SMC_D6); + sam_configgpio(GPIO_SMC_D7); + sam_configgpio(GPIO_SMC_D8); + sam_configgpio(GPIO_SMC_D9); + sam_configgpio(GPIO_SMC_D10); + sam_configgpio(GPIO_SMC_D11); + sam_configgpio(GPIO_SMC_D12); + sam_configgpio(GPIO_SMC_D13); + sam_configgpio(GPIO_SMC_D14); + sam_configgpio(GPIO_SMC_D15); + + /* SAME70 SDRAM + * --------------- ----------- + * PC20 A2 A0 + * PC21 A3 A1 + * PC22 A4 A2 + * PC23 A5 A3 + * PC24 A6 A4 + * PC25 A7 A5 + * PC26 A8 A6 + * PC27 A9 A7 + * PC28 A10 A8 + * PC29 A11 A9 + * PD13 SDA10 A10 + * PA20 BA0 A11 + * PD17 CAS nCAS + * PD14 SDCKE CKE + * PD23 SDCK CLK + * PC15 SDCS nCS + * PC18 A0/NBS0 LDQM + * PD16 RAS nRAS + * PD15 NWR1/NBS1 UDQM + * PD29 SDWE nWE + */ + + sam_configgpio(GPIO_SMC_A2); /* PC20 A2 -> A0 */ + sam_configgpio(GPIO_SMC_A3); /* PC21 A3 -> A1 */ + sam_configgpio(GPIO_SMC_A4); /* PC22 A4 -> A2 */ + sam_configgpio(GPIO_SMC_A5); /* PC23 A5 -> A3 */ + sam_configgpio(GPIO_SMC_A6); /* PC24 A6 -> A4 */ + sam_configgpio(GPIO_SMC_A7); /* PC25 A7 -> A5 */ + sam_configgpio(GPIO_SMC_A8); /* PC26 A8 -> A6 */ + sam_configgpio(GPIO_SMC_A9); /* PC27 A9 -> A7 */ + sam_configgpio(GPIO_SMC_A10); /* PC28 A10 -> A8 */ + sam_configgpio(GPIO_SMC_A11); /* PC29 A11 -> A9 */ + sam_configgpio(GPIO_SDRAMC_A10_2); /* PD13 SDA10 -> A10 */ + sam_configgpio(GPIO_SDRAMC_BA0); /* PA20 BA0 -> A11 */ + + sam_configgpio(GPIO_SDRAMC_CKE); /* PD14 SDCKE -> CKE */ + sam_configgpio(GPIO_SDRAMC_CK); /* PD23 SDCK -> CLK */ + sam_configgpio(GPIO_SDRAMC_CS_1); /* PC15 SDCS -> nCS */ + sam_configgpio(GPIO_SDRAMC_RAS); /* PD16 RAS -> nRAS */ + sam_configgpio(GPIO_SDRAMC_CAS); /* PD17 CAS -> nCAS */ + sam_configgpio(GPIO_SDRAMC_WE); /* PD29 SDWE -> nWE */ + sam_configgpio(GPIO_SMC_NBS0); /* PC18 A0/NBS0 -> LDQM */ + sam_configgpio(GPIO_SMC_NBS1); /* PD15 NWR1/NBS1 -> UDQM */ + + /* Enable the SDRAMC peripheral */ + + sam_sdramc_enableclk(); + + regval = getreg32(SAM_MATRIX_CCFG_SMCNFCS); + regval |= MATRIX_CCFG_SMCNFCS_SDRAMEN; + putreg32(regval, SAM_MATRIX_CCFG_SMCNFCS); + + /* 1. SDRAM features must be set in the configuration register: + * asynchronous timings (TRC, TRAS, etc.), number of columns, rows, CAS + * latency, and the data bus width. + * + * SDRAMC_CR_NC_COL8 8 column bits + * SDRAMC_CR_NR_ROW11 1 row bits + * SDRAMC_CR_NB_BANK2 2 banks + * SDRAMC_CR_CAS_LATENCY3 3 cycle CAS latency + * SDRAMC_CR_DBW 16 bit + * SDRAMC_CR_TWR(4) 4 cycle write recovery delay + * SDRAMC_CR_TRCTRFC(11) 63 ns min + * SDRAMC_CR_TRP(5) 21 ns min Command period (PRE to ACT) + * SDRAMC_CR_TRCD(5) 21 ns min Active Command to read/Write Command delay time + * SDRAMC_CR_TRAS(8) 42 ns min Command period (ACT to PRE) + * SDRAMC_CR_TXSR(13) 70 ns min Exit self-refresh to active time + */ + + regval = SDRAMC_CR_NC_COL8 | /* 8 column bits */ + SDRAMC_CR_NR_ROW11 | /* 11 row bits */ + SDRAMC_CR_NB_BANK2 | /* 2 banks */ + SDRAMC_CR_CAS_LATENCY3 | /* 3 cycle CAS latency */ + SDRAMC_CR_DBW | /* 16 bit */ + SDRAMC_CR_TWR(4) | /* 4 cycle write recovery delay */ + SDRAMC_CR_TRCTRFC(11) | /* 63 ns min */ + SDRAMC_CR_TRP(5) | /* 21 ns min Command period (PRE to ACT) */ + SDRAMC_CR_TRCD(5) | /* 21 ns min Active Command to read/Write Command delay time */ + SDRAMC_CR_TRAS(8) | /* 42 ns min Command period (ACT to PRE) */ + SDRAMC_CR_TXSR(13); /* 70 ns min Exit self-refresh to active time */ + + putreg32(regval, SAM_SDRAMC_CR); + + /* 2. For mobile SDRAM, temperature-compensated self refresh (TCSR), drive + * strength (DS) and partial array self refresh (PASR) must be set in + * the Low Power Register. + */ + + putreg32(0, SAM_SDRAMC_LPR); + + /* 3. The SDRAM memory type must be set in the Memory Device Register.*/ + + putreg32(SDRAMC_MDR_SDRAM, SAM_SDRAMC_MDR); + + /* 4. A minimum pause of 200 usec is provided to precede any signal toggle.*/ + + up_udelay(200); + + /* 5. A NOP command is issued to the SDRAM devices. The application must + * set Mode to 1 in the Mode Register and perform a write access to any + * SDRAM address. + */ + + putreg32(SDRAMC_MR_MODE_NOP, SAM_SDRAMC_MR); + *psdram = 0; + up_udelay(200); + + /* 6. An All Banks Precharge command is issued to the SDRAM devices. The + * application must set Mode to 2 in the Mode Register and perform a + * write access to any SDRAM address. + */ + + putreg32(SDRAMC_MR_MODE_PRECHARGE, SAM_SDRAMC_MR); + *psdram = 0; + up_udelay(200); + + /* 7. Eight auto-refresh (CBR) cycles are provided. The application must + * set the Mode to 4 in the Mode Register and perform a write access to + * any SDRAM location eight times. + */ + + for (i = 0 ; i < 8; i++) { + putreg32(SDRAMC_MR_MODE_AUTOREFRESH, SAM_SDRAMC_MR); + *psdram = 0; + } + + up_udelay(200); + + /* 8. A Mode Register set (MRS) cycle is issued to program the parameters + * of the SDRAM devices, in particular CAS latency and burst length. + * The application must set Mode to 3 in the Mode Register and perform + * a write access to the SDRAM. The write address must be chosen so + * that BA[1:0] are set to 0. For example, with a 16-bit 128 MB SDRAM + * (12 rows, 9 columns, 4 banks) bank address, the SDRAM write access + * should be done at the address 0x70000000. + */ + + putreg32(SDRAMC_MR_MODE_LOADMODE, SAM_SDRAMC_MR); + *psdram = 0; + up_udelay(200); + + /* 9. For mobile SDRAM initialization, an Extended Mode Register set + * (EMRS) cycle is issued to program the SDRAM parameters (TCSR, PASR, + * DS). The application must set Mode to 5 in the Mode Register and + * perform a write access to the SDRAM. The write address must be + * chosen so that BA[1] or BA[0] are set to 1. + * + * For example, with a 16-bit 128 MB SDRAM, (12 rows, 9 columns, 4 + * banks) bank address the SDRAM write access should be done at the + * address 0x70800000 or 0x70400000. + */ + + //putreg32(SDRAMC_MR_MODE_EXTLOADMODE, SDRAMC_MR_MODE_EXT_LOAD_MODEREG); + // *((uint8_t *)(psdram + SDRAM_BA0)) = 0; + + /* 10. The application must go into Normal Mode, setting Mode to 0 in the + * Mode Register and performing a write access at any location in the + * SDRAM. + */ + + putreg32(SDRAMC_MR_MODE_NORMAL, SAM_SDRAMC_MR); + *psdram = 0; + up_udelay(200); + + /* 11. Write the refresh rate into the count field in the SDRAMC Refresh + * Timer register. (Refresh rate = delay between refresh cycles). The + * SDRAM device requires a refresh every 15.625 usec or 7.81 usec. With + * a 100 MHz frequency, the Refresh Timer Counter Register must be set + * with the value 1562(15.625 usec x 100 MHz) or 781(7.81 usec x 100 + * MHz). + * + * For IS42S16100E, 2048 refresh cycle every 32ms, every 15.625 usec + */ + + regval = (32 * (BOARD_MCK_FREQUENCY / 1000)) / 2048 ; + putreg32(regval, SAM_SDRAMC_TR); + + regval = getreg32(SAM_SDRAMC_CFR1); + regval |= SDRAMC_CFR1_UNAL; + putreg32(regval, SAM_SDRAMC_CFR1); + + /* After initialization, the SDRAM devices are fully functional. */ +} + +#endif /* CONFIG_SAMV7_SDRAMC */ diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_spi.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_spi.c new file mode 100644 index 0000000000..41d2d36f05 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_spi.c @@ -0,0 +1,241 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4same70xplained_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: boad_spi_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4_SAME70XPLAINED_V1 board. + * + ************************************************************************************/ + +__EXPORT void board_spi_initialize(void) +{ +#ifdef CONFIG_SAMV7_SPI0_MASTER + sam_configgpio(GPIO_SPI_CS_GYRO); + sam_configgpio(GPIO_SPI_CS_ACCEL_MAG); + sam_configgpio(GPIO_SPI_CS_BARO); + sam_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + sam_gpiowrite(GPIO_SPI_CS_GYRO, 1); + sam_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + sam_gpiowrite(GPIO_SPI_CS_BARO, 1); + sam_gpiowrite(GPIO_SPI_CS_MPU, 1); + + sam_configgpio(GPIO_EXTI_GYRO_DRDY); + sam_configgpio(GPIO_EXTI_MAG_DRDY); + sam_configgpio(GPIO_EXTI_ACCEL_DRDY); + sam_configgpio(GPIO_EXTI_MPU_DRDY); +#endif + +#ifdef CONFIG_SAMV7_SPI1_MASTER + sam_configgpio(GPIO_SPI_CS_EXT0); + sam_configgpio(GPIO_SPI_CS_EXT1); + sam_configgpio(GPIO_SPI_CS_EXT2); + sam_configgpio(GPIO_SPI_CS_EXT3); + sam_gpiowrite(GPIO_SPI_CS_EXT0, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT1, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT2, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT3, 1); +#endif +} + +/**************************************************************************** + * Name: sam_spi[0|1]select + * + * Description: + * PIO chip select pins may be programmed by the board specific logic in + * one of two different ways. First, the pins may be programmed as SPI + * peripherals. In that case, the pins are completely controlled by the + * SPI driver. This method still needs to be provided, but it may be only + * a stub. + * + * An alternative way to program the PIO chip select pins is as a normal + * PIO output. In that case, the automatic control of the CS pins is + * bypassed and this function must provide control of the chip select. + * NOTE: In this case, the PIO output pin does *not* have to be the + * same as the NPCS pin normal associated with the chip select number. + * + * Input Parameters: + * devid - Identifies the (logical) device + * selected - TRUE:Select the device, FALSE:De-select the device + * + * Returned Values: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_SAMV7_SPI0_MASTER +void sam_spi0select(uint32_t devid, bool selected) +{ + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + sam_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + sam_gpiowrite(GPIO_SPI_CS_BARO, 1); + sam_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_GYRO, 1); + sam_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + sam_gpiowrite(GPIO_SPI_CS_BARO, 1); + sam_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_GYRO, 1); + sam_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + sam_gpiowrite(GPIO_SPI_CS_BARO, !selected); + sam_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_GYRO, 1); + sam_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + sam_gpiowrite(GPIO_SPI_CS_BARO, 1); + sam_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + } +} +#endif + +#ifdef CONFIG_SAMV7_SPI1_MASTER +void sam_spi1select(uint32_t devid, bool selected) +{ + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + sam_gpiowrite(GPIO_SPI_CS_EXT1, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT2, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_EXT0, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + sam_gpiowrite(GPIO_SPI_CS_EXT2, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT2: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_EXT0, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT1, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + sam_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT3: + /* Making sure the other peripherals are not selected */ + sam_gpiowrite(GPIO_SPI_CS_EXT0, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT1, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT2, 1); + sam_gpiowrite(GPIO_SPI_CS_EXT3, !selected); + break; + + default: + break; + + } +} +#endif + +/**************************************************************************** + * Name: sam_spi[0|1]status + * + * Description: + * Return status information associated with the SPI device. + * + * Input Parameters: + * devid - Identifies the (logical) device + * + * Returned Values: + * Bit-encoded SPI status (see include/nuttx/spi/spi.h. + * + ****************************************************************************/ + +#ifdef CONFIG_SAMV7_SPI0_MASTER +uint8_t sam_spi0status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_SAMV7_SPI1_MASTER +uint8_t sam_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_timer_config.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_timer_config.c new file mode 100644 index 0000000000..90612cbe3e --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_timer_config.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file px4same70xplained_timer_config.c + * + * Configuration data for the sam7 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = 0, + .clock_register = 0, + .clock_bit = 0, + .clock_freq = 0, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = 0, + + }, +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = 0,//GPIO_TIM1_CH4OUT, + .gpio_in = 0,//GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = 0, + .masks = 0 + }, +}; diff --git a/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_usb.c b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_usb.c new file mode 100644 index 0000000000..ebf1fe4934 --- /dev/null +++ b/src/drivers/boards/px4-same70xplained-v1/px4same70xplained_usb.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4same70xplained_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include "sam_gpio.h" +#include "sam_usbdev.h" +#include "board_config.h" + + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_usb_initialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4_SAME70XPLAINED_V1 board. + * + ************************************************************************************/ + +__EXPORT void board_usb_initialize(void) +{ + /* Initialize the VBUS enable signal to HI output in any event so that, by + * default, VBUS power is not provided at the USB connector. + */ + + sam_configgpio(GPIO_VBUSON); +} + +/*********************************************************************************** + * Name: sam_usbsuspend + * + * Description: + * Board logic must provide the sam_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. + * + ************************************************************************************/ + +void sam_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/src/drivers/samv7/CMakeLists.txt b/src/drivers/samv7/CMakeLists.txt new file mode 100644 index 0000000000..e5ab12ddf4 --- /dev/null +++ b/src/drivers/samv7/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__samv7 + COMPILE_FLAGS + -Os + SRCS + drv_hrt.c + drv_io_timer.c + drv_pwm_servo.c + drv_input_capture.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/samv7/adc/CMakeLists.txt b/src/drivers/samv7/adc/CMakeLists.txt new file mode 100644 index 0000000000..e9b4e86209 --- /dev/null +++ b/src/drivers/samv7/adc/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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 drivers__samv7__adc + MAIN adc + COMPILE_FLAGS + -Os + SRCS + adc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/samv7/adc/adc.cpp b/src/drivers/samv7/adc/adc.cpp new file mode 100644 index 0000000000..5137f57f82 --- /dev/null +++ b/src/drivers/samv7/adc/adc.cpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 adc.cpp + * + * Driver for the STM32 ADC. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADC driver. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#if defined(ADC_CHANNELS) +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +#ifdef STM32_ADC_CCR +# define rCCR REG(STM32_ADC_CCR_OFFSET) +#endif + +class ADC : public device::CDev +{ +public: + ADC(uint32_t channels); + ~ADC(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t read(file *filp, char *buffer, size_t len); + +protected: + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + +private: + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + + hrt_call _call; + perf_counter_t _sample_perf; + + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + orb_advert_t _to_system_power; + + /** work trampoline */ + static void _tick_trampoline(void *arg); + + /** worker function */ + void _tick(); + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ + uint16_t _sample(unsigned channel); + + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); +}; + +ADC::ADC(uint32_t channels) : + CDev("adc", ADC0_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), + _channel_count(0), + _samples(nullptr), + _to_system_power(nullptr) +{ + _debug_enabled = true; + + /* always enable the temperature sensor */ + channels |= 1 << 16; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } +} + +ADC::~ADC() +{ + if (_samples != nullptr) { + delete _samples; + } +} + +int +ADC::init() +{ + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_CAL; + usleep(100); + + if (rCR2 & ADC_CR2_CAL) { + return -1; + } + +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2 |= ADC_CR2_SWSTART; + + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + DEVICE_LOG("sample timeout"); + return -1; + } + } + + + DEVICE_DEBUG("init done"); + + /* create the device node */ + return CDev::init(); +} + +int +ADC::ioctl(file *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADC::read(file *filp, char *buffer, size_t len) +{ + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) { + len = maxsize; + } + + /* block interrupts while copying samples to avoid racing with an update */ + irqstate_t flags = enter_critical_section(); + memcpy(buffer, _samples, len); + leave_critical_section(flags); + + return len; +} + +int +ADC::open_first(struct file *filp) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADC::close_last(struct file *filp) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADC::_tick_trampoline(void *arg) +{ + (reinterpret_cast(arg))->_tick(); +} + +void +ADC::_tick() +{ + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) { + _samples[i].am_data = _sample(_samples[i].am_channel); + } + + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + +uint16_t +ADC::_sample(unsigned channel) +{ + perf_begin(_sample_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) { + rSR &= ~ADC_SR_EOC; + } + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + DEVICE_LOG("sample timeout"); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(_sample_perf); + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + + int fd = open(ADC0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "can't open ADC device"); + } + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[12]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) { + errx(1, "read error"); + } + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ + g_adc = new ADC(ADC_CHANNELS); + + if (g_adc == nullptr) { + errx(1, "couldn't allocate the ADC driver"); + } + + if (g_adc->init() != OK) { + delete g_adc; + errx(1, "ADC init failed"); + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) { + test(); + } + } + + exit(0); +} +#endif diff --git a/src/drivers/samv7/adc/module.mk b/src/drivers/samv7/adc/module.mk new file mode 100644 index 0000000000..38ea490a0e --- /dev/null +++ b/src/drivers/samv7/adc/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# STM32 ADC driver +# + +MODULE_COMMAND = adc + +SRCS = adc.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/samv7/drv_hrt.c b/src/drivers/samv7/drv_hrt.c new file mode 100644 index 0000000000..9774dac6fa --- /dev/null +++ b/src/drivers/samv7/drv_hrt.c @@ -0,0 +1,971 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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 drv_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * This can use any general or advanced STM32 timer. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX STM32 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include +#include "sam_gpio.h" +#include "sam_tc.h" + +#ifdef HRT_TIMER_CHANNEL + +#if defined(HRT_TIMER) +# error "HRT_TIMER should not be defined, instead define HRT_TIMER_CHANNEL from 0-11" +#endif + +#if HRT_TIMER_CHANNEL == 0 || HRT_TIMER_CHANNEL == 1 || HRT_TIMER_CHANNEL == 2 +# define HRT_TIMER 0 +#endif +#if HRT_TIMER_CHANNEL == 3 || HRT_TIMER_CHANNEL == 4 || HRT_TIMER_CHANNEL == 5 +# define HRT_TIMER 1 +#endif +#if HRT_TIMER_CHANNEL == 6 || HRT_TIMER_CHANNEL == 7 || HRT_TIMER_CHANNEL == 8 +# define HRT_TIMER 2 +#endif +#if HRT_TIMER_CHANNEL == 9 || HRT_TIMER_CHANNEL == 10 || HRT_TIMER_CHANNEL == 11 +# define HRT_TIMER 3 +#endif + +/* HRT configuration */ +#if HRT_TIMER == 0 +# define HRT_TIMER_BASE SAM_TC012_BASE +# if !defined(CONFIG_SAMV7_TC0) +# error "HRT_TIMER_CHANNEL 0-2 Require CONFIG_SAMV7_TC0=y" +# endif +#elif HRT_TIMER == 1 +# define HRT_TIMER_BASE SAM_TC345_BASE +# if !defined(CONFIG_SAMV7_TC1) +# error "HRT_TIMER_CHANNEL 3-5 Require CONFIG_SAMV7_TC1=y" +# endif +#elif HRT_TIMER == 2 +# define HRT_TIMER_BASE SAM_TC678_BASE +# if !defined(CONFIG_SAMV7_TC2) +# error "HRT_TIMER_CHANNEL 6-8 Require CONFIG_SAMV7_TC2=y" +# endif +#elif HRT_TIMER == 3 +# define HRT_TIMER_BASE SAM_TC901_BASE +# if !defined(CONFIG_SAMV7_TC3) +# error "HRT_TIMER_CHANNEL 9-11 Require CONFIG_SAMV7_TC3=y" +# endif +#else +# error "HRT_TIMER_CHANNEL should be defined valid value are from 0-11" +# endif +/* + * We are going to request 1 1Mhz input to our counter + * but on this platform we will most likely get a hotter value + * We will test this by assertion. + */ +# define HRT_TIMER_CLOCK (BOARD_MCK_FREQUENCY/128) +# define HRT_DESIRED_FREQUENCY 1000000 + +/* + * HRT clock must be a greater than 1MHz + */ + +#if HRT_TIMER_CLOCK <= 1000000 +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Scaling factor for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_TIME_SCALE(_t) ((((_t) * HRT_TIMER_CLOCK) / USEC_PER_SEC) + 1) + +/* + * Scaling factor for the free-running counter; convert an input + * in time in microseconds to counts. + */ +#define HRT_COUNTER_SCALE(_c) ((_c) * USEC_PER_SEC) / HRT_TIMER_CLOCK + +/** + * Minimum/maximum deadlines. + * + * These are suitable for use with a 16-bit timer/counter clocked + * at 1MHz. The high-resolution timer need only guarantee that it + * not wrap more than once in the 50ms period for absolute time to + * be consistently maintained. + * + * The minimum deadline must be such that the time taken between + * reading a time and writing a deadline to the timer cannot + * result in missing the deadline. + */ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* + * Period of the free-running counter, in microseconds. + */ +#define HRT_COUNTER_PERIOD 65536 + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(SAM_TC0_BASE + SAM_TC_CHAN_OFFSET(HRT_TIMER_CHANNEL) + _reg)) + +#define rCCR REG(SAM_TC_CCR_OFFSET) +#define rCMR REG(SAM_TC_CMR_OFFSET) +#define rSMMR REG(SAM_TC_SMMR_OFFSET) +#define rRAB REG(SAM_TC_RAB_OFFSET) +#define rCV REG(SAM_TC_CV_OFFSET) +#define rRA REG(SAM_TC_RA_OFFSET) +#define rRB REG(SAM_TC_RB_OFFSET) +#define rRC REG(SAM_TC_RC_OFFSET) +#define rSR REG(SAM_TC_SR_OFFSET) +#define rIER REG(SAM_TC_IER_OFFSET) +#define rIDR REG(SAM_TC_IDR_OFFSET) +#define rIMR REG(SAM_TC_IMR_OFFSET) +#define rEMR REG(SAM_TC_EMR_OFFSET) +/* +* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + + +/* timer-specific functions */ +static void hrt_tim_init(void); +static void hrt_tim_isr(TC_HANDLE tch, void *arg, uint32_t sr); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/* + * Specific registers and bits used by PPM sub-functions + */ +#undef HRT_PPM_CHANNEL //todo:implement PPM +#ifdef HRT_PPM_CHANNEL + +# if HRT_PPM_CHANNEL == 1 +# define rCCR_PPM rCCR /* capture register for PPM */ +# define DIER_PPM TIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 1 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC1P +# elif HRT_PPM_CHANNEL == 2 +# define rCCR_PPM rCCR2 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 2 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC2P +# elif HRT_PPM_CHANNEL == 3 +# define rCCR_PPM rCCR3 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 1 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC3P +# elif HRT_PPM_CHANNEL == 4 +# define rCCR_PPM rCCR4 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 2 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC4P +# else +# error HRT_PPM_CHANNEL must be a value between 1 and 4 +# endif + +/* + * PPM decoder tuning parameters + */ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 5 +#define PPM_MAX_CHANNELS 20 + +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ + +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; +__EXPORT unsigned ppm_decoded_channels = 0; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +#define PPM_DEBUG 0 + +#if PPM_DEBUG +/* PPM edge history */ +__EXPORT uint16_t ppm_edge_history[32]; +unsigned ppm_edge_next; + +/* PPM pulse history */ +__EXPORT uint16_t ppm_pulse_history[32]; +unsigned ppm_pulse_next; +#endif + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/** PPM decoder state machine */ +struct { + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +static void hrt_ppm_decode(uint32_t status); + +#else +/* disable the PPM configuration */ +# define rCCR_PPM 0 +# define DIER_PPM 0 +# define SR_INT_PPM 0 +# define SR_OVF_PPM 0 +# define CCMR1_PPM 0 +# define CCMR2_PPM 0 +# define CCER_PPM 0 +#endif /* HRT_PPM_CHANNEL */ + +/** + * Initialise the timer we are going to use. + * + * We expect that we'll own one of the reduced-function STM32 general + * timers, and that we can use channel 1 in compare mode. + */ + +static TC_HANDLE hrt_tch = (TC_HANDLE) - 1; +static void +hrt_tim_init(void) +{ + uint32_t frequency; + uint32_t actual; + uint32_t cmr; + int ret; + + /* We would like 1Mhz count rate */ + + frequency = HRT_DESIRED_FREQUENCY; + + /* The pre-calculate values to use when we start the timer */ + + ret = sam_tc_clockselect(frequency, &cmr, &actual); + + if (ret < 0) { + PX4_PANIC("ERROR: Failed no divisor can be found (%d),for timer channel %d\n", ret, HRT_TIMER_CHANNEL); + return; + } + + /* Verify that what we got was what we expected */ + + ASSERT(actual == HRT_TIMER_CLOCK); + + /* Allocate the timer/counter and select its mode of operation + * + * TC_CMR_TCCLKS - Returned by sam_tc_clockselect + * TC_CMR_CLKI=0 - Not inverted + * TC_CMR_BURST_NONE - Not gated by an external signal + * TC_CMR_CPCSTOP=0 - Do not Stop the clock on an RC compare event + * TC_CMR_CPCDIS=0 - Don't disable the clock on an RC compare event + * TC_CMR_EEVTEDG_NONE - No external events (and, hence, no edges + * TC_CMR_EEVT_TIOB - ???? REVISIT + * TC_CMR_ENET=0 - External event trigger disabled + * TC_CMR_WAVSEL_UP - TC_CV is incremented from 0 to 65535 + * TC_CMR_WAVE - Waveform mode + * TC_CMR_ACPA_NONE - RA compare has no effect on TIOA + * TC_CMR_ACPC_NONE - RC compare has no effect on TIOA + * TC_CMR_AEEVT_NONE - No external event effect on TIOA + * TC_CMR_ASWTRG_NONE - No software trigger effect on TIOA + * TC_CMR_BCPB_NONE - RB compare has no effect on TIOB + * TC_CMR_BCPC_NONE - RC compare has no effect on TIOB + * TC_CMR_BEEVT_NONE - No external event effect on TIOB + * TC_CMR_BSWTRG_NONE - No software trigger effect on TIOB + */ + + cmr |= (TC_CMR_BURST_NONE | TC_CMR_EEVTEDG_NONE | TC_CMR_EEVT_TIOB | + TC_CMR_WAVSEL_UP | TC_CMR_WAVE | TC_CMR_ACPA_NONE | + TC_CMR_ACPC_NONE | TC_CMR_AEEVT_NONE | TC_CMR_ASWTRG_NONE | + TC_CMR_BCPB_NONE | TC_CMR_BCPC_NONE | TC_CMR_BEEVT_NONE | + TC_CMR_BSWTRG_NONE); + + hrt_tch = sam_tc_allocate(HRT_TIMER_CHANNEL, cmr); + + if (!hrt_tch) { + PX4_PANIC("ERROR: Failed to allocate timer channel %d\n", HRT_TIMER_CHANNEL); + return; + } + + irqstate_t flags = enter_critical_section(); + + /* Start with 1 ms */ + + uint64_t regval = HRT_TIME_SCALE(1000LL); + + ASSERT(regval <= UINT16_MAX); + + /* Set up to receive the callback when the Compare interrupt occurs */ + + (void)sam_tc_attach(hrt_tch, hrt_tim_isr, hrt_tch, TC_INT_CPCS); + + /* Set RC so that an event will be triggered when TC_CV register counts + * up to RC. + */ + + rRC = regval & UINT16_MAX; + + /* Start the counter */ + + sam_tc_start(hrt_tch); + + /* Enable interrupts. We should get the callback when the interrupt + * occurs. + */ + + leave_critical_section(flags); +} + +#ifdef HRT_PPM_CHANNEL +/** + * Handle the PPM decoder state machine. + */ +static void +hrt_ppm_decode(uint32_t status) +{ + uint16_t count = rCCR_PPM; + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PPM) { + goto error; + } + + /* how long since the last edge? - this handles counter wrapping implicitely. */ + width = count - ppm.last_edge; + +#if PPM_DEBUG + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) { + ppm_edge_next = 0; + } + +#endif + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) { + ppm_buffer[i] = ppm_temp_buffer[i]; + } + + ppm_last_valid_decode = hrt_absolute_time(); + + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + ppm.last_edge = count; + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + break; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; + + case INACTIVE: + + /* we expect a short pulse */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + break; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + +#if PPM_DEBUG + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) { + ppm_pulse_next = 0; + } + +#endif + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { + goto error; + } + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) { + ppm_temp_buffer[ppm.next_channel++] = interval; + } + + ppm.phase = INACTIVE; + break; + + } + + ppm.last_edge = count; + return; + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + +} +#endif /* HRT_PPM_CHANNEL */ + +/** + * Handle the compare interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +void hrt_tim_isr(TC_HANDLE tch, void *arg, uint32_t sr) +{ + /* grab the timer for latency tracking purposes */ + + latency_actual = rCV; + +#ifdef HRT_PPM_CHANNEL + + /* was this a PPM edge? */ + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + + hrt_ppm_decode(status); + } + +#endif + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); +} + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime; + uint32_t count; + irqstate_t flags; + + /* + * Counter state. Marked volatile as they may change + * inside this routine but outside the irqsave/restore + * pair. Discourage the compiler from moving loads/stores + * to these outside of the protected range. + */ + static volatile hrt_abstime base_time; + static volatile uint32_t last_count; + + /* prevent re-entry */ + flags = enter_critical_section(); + + /* get the current counter value */ + count = rCV; + + /* + * Determine whether the counter has wrapped since the + * last time we're called. + * + * This simple test is sufficient due to the guarantee that + * we are always called at least once per counter period. + */ + if (count < last_count) { + base_time += HRT_COUNTER_PERIOD; + } + + /* save the count for next time */ + last_count = count; + + /* compute the current time */ + abstime = HRT_COUNTER_SCALE(base_time + count); + + leave_critical_section(flags); + + return abstime; +} + +/** + * Convert a timespec to absolute time + */ +hrt_abstime +ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/** + * Convert absolute time to a timespec. + */ +void +abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +/** + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = enter_critical_section(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + leave_critical_section(flags); + + return delta; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = enter_critical_section(); + + hrt_abstime ts = hrt_absolute_time(); + + leave_critical_section(flags); + + return ts; +} + +/** + * Initalise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef HRT_PPM_CHANNEL + /* configure the PPM input pin */ + stm32_configgpio(GPIO_PPM_IN); +#endif +} + +/** + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/** + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/** + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + leave_critical_section(flags); +} + +/** + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/** + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = enter_critical_section(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + leave_critical_section(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } + + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + rRC = latency_baseline = HRT_TIME_SCALE(deadline) & UINT16_MAX; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +#endif /* HRT_TIMER_CHANNEL */ diff --git a/src/drivers/samv7/drv_input_capture.c b/src/drivers/samv7/drv_input_capture.c new file mode 100644 index 0000000000..ffe9fcdbc8 --- /dev/null +++ b/src/drivers/samv7/drv_input_capture.c @@ -0,0 +1,396 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_input_capture.c + * + * Servo driver supporting input capture connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have input pins. + * + * Require an interrupt. + * + * The use of thie interface is mutually exclusive with the pwm + * because the same timers are used and there is a resource contention + * with the ARR as it sets the pwm rate and in this driver needs to match + * that of the hrt to back calculate the actual point in time the edge + * was detected. + * + * This is accomplished by taking the difference between the current + * count rCNT snapped at the time interrupt and the rCCRx captured on the + * edge transition. This delta is applied to hrt time and the resulting + * value is the absolute system time the edge occured. + * + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "drv_io_timer.h" + +#include "drv_input_capture.h" + +#include +#include +#include + +static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + capture_callback_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + + +static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt) +{ +} +static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context) +{ + irqstate_t flags = enter_critical_section(); + channel_handlers[channel].callback = callback; + channel_handlers[channel].context = context; + leave_critical_section(flags); +} + +static void input_capture_unbind(unsigned channel) +{ + input_capture_bind(channel, NULL, NULL); +} + +int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, + capture_callback_t callback, void *context) +{ + if (filter > 200) {//GTIM_CCMR1_IC1F_MASK) { + return -EINVAL; + } + + if (edge > Both) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (edge == Disabled) { + + io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); + input_capture_unbind(channel); + + } else { + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + input_capture_bind(channel, callback, context); + + rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); + + if (rv != 0) { + return rv; + } + + rv = up_input_capture_set_filter(channel, filter); + + if (rv == 0) { + rv = up_input_capture_set_trigger(channel, edge); + + if (rv == 0) { + rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel); + } + } + } + } + + return rv; +} + + + +int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + uint32_t timer = timer_io_channels[channel].timer_index; + rv = OK; + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + case 2: + case 3: + case 4: + default: + UNUSED(timer); + rv = -EIO; + } + } + } + + return rv; +} +int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + uint32_t timer = timer_io_channels[channel].timer_index; + + irqstate_t flags = enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + case 2: + case 3: + case 4: + default: + UNUSED(timer); + rv = -EIO; + } + + leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + + uint32_t timer = timer_io_channels[channel].timer_index; + uint32_t rvalue = 0; + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + case 2: + case 3: + case 4: + default: + UNUSED(timer); + rv = -EIO; + } + + if (rv == 0) { + switch (rvalue) { + + case 0: + *edge = Rising; + break; + + case 1:// (GTIM_CCER_CC1P | GTIM_CCER_CC1NP): + *edge = Both; + break; + + case 2: //(GTIM_CCER_CC1P): + *edge = Falling; + break; + + default: + rv = -EIO; + } + } + } + } + + return rv; +} +int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + uint16_t edge_bits = 0xffff; + + switch (edge) { + case Disabled: + break; + + case Rising: + edge_bits = 0; + break; + + case Falling: + edge_bits = 0;// GTIM_CCER_CC1P; + break; + + case Both: + edge_bits = 0;// GTIM_CCER_CC1P | GTIM_CCER_CC1NP; + break; + + default: + return -EINVAL;; + } + + uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + rv = OK; + + irqstate_t flags = enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + case 2: + case 3: + case 4: + default: + UNUSED(rvalue); + UNUSED(timer); + UNUSED(edge_bits); + rv = -EIO; + } + + leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + irqstate_t flags = enter_critical_section(); + *callback = channel_handlers[channel].callback; + *context = channel_handlers[channel].context; + leave_critical_section(flags); + rv = OK; + } + } + + return rv; + +} + +int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + input_capture_bind(channel, callback, context); + rv = 0; + } + } + + return rv; +} + +int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + irqstate_t flags = enter_critical_section(); + *stats = channel_stats[channel]; + + if (clear) { + memset(&channel_stats[channel], 0, sizeof(*stats)); + } + + leave_critical_section(flags); + } + + return rv; +} diff --git a/src/drivers/samv7/drv_input_capture.h b/src/drivers/samv7/drv_input_capture.h new file mode 100644 index 0000000000..968ce831ef --- /dev/null +++ b/src/drivers/samv7/drv_input_capture.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_input_capture.h + * + * stm32-specific input capture data. + */ + +#pragma once + +#include diff --git a/src/drivers/samv7/drv_io_timer.c b/src/drivers/samv7/drv_io_timer.c new file mode 100644 index 0000000000..c3f53158bd --- /dev/null +++ b/src/drivers/samv7/drv_io_timer.c @@ -0,0 +1,869 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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 drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to SAMV7 timer blocks. + * + * todo:This File is stubbed out! And need to be implemented + * + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drv_io_timer.h" + +#include +#include +#include + +#include +#include +#include + +/* If the timer clock source provided as clock_freq is the TBD + * then configure the timer to free-run at 1MHz. + * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. + * For instance .clock_freq = 1000000 would set the prescaler to 1. + * We also allow for overrides here but all timer register usage need to be + * taken into account + */ +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 1000000 +#endif + +#if !defined(BOARD_ONESHOT_FREQ) +#define BOARD_ONESHOT_FREQ 8000000 +#endif + +#define MAX_CHANNELS_PER_TIMER 4 + +// NotUsed PWMOut PWMIn Capture OneShot Trigger +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0, 0, 0 }; + +typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ + +static io_timer_allocation_t once = 0; +#if defined(BOARD_HAS_CAPTURE) + +/* Stats and handlers are only useful for Capture */ + +typedef struct channel_stat_t { + uint32_t isr_cout; + uint32_t overflows; +} channel_stat_t; + +static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + channel_handler_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; +#endif // defined(BOARD_HAS_CAPTURE) + +static int io_timer_handler(uint16_t timer_index) +{ +#if defined(BOARD_HAS_CAPTURE) + /* Read the count at the time of the interrupt */ + + uint16_t count = 0; //rCNT(timer_index); + + /* Read the HRT at the time of the interrupt */ + + hrt_abstime now = hrt_absolute_time(); + + const io_timers_t *tmr = &io_timers[timer_index]; + + /* What is pending and enabled? */ + + uint16_t statusr = 0; //rSR(timer_index); + uint16_t enabled = 0; //rDIER(timer_index) & GTIM_SR_CCIF; + + /* Iterate over the timer_io_channels table */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + + uint16_t masks = timer_io_channels[chan_index].masks; + + /* Do we have an enabled channel */ + + if (enabled & masks) { + + + if (statusr & masks & 0 /*GTIM_SR_CCIF*/) { + + io_timer_channel_stats[chan_index].isr_cout++; + + /* Call the client to read the CCxR etc and clear the CCxIF */ + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, + chan_index, &timer_io_channels[chan_index], + now, count); + } + } + + if (statusr & masks & 0 /*GTIM_SR_CCOF*/) { + + /* Error we has a second edge before we cleared CCxR */ + + io_timer_channel_stats[chan_index].overflows++; + } + } + } + + /* Clear all the SR bits for interrupt enabled channels only */ + + //rSR(timer_index) = ~(statusr & (enabled | enabled << 8)); +#endif // defined(BOARD_HAS_CAPTURE) + return 0; +} + +int io_timer_handler0(int irq, void *context, void *arg) +{ + + return io_timer_handler(0); +} + +int io_timer_handler1(int irq, void *context, void *arg) +{ + return io_timer_handler(1); + +} + +int io_timer_handler2(int irq, void *context, void *arg) +{ + return io_timer_handler(2); + +} + +int io_timer_handler3(int irq, void *context, void *arg) +{ + return io_timer_handler(3); + +} + +static inline int validate_timer_index(unsigned timer) +{ + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +static inline int is_timer_uninitalized(unsigned timer) +{ + int rv = 0; + + if (once & 1 << timer) { + rv = -EBUSY; + } + + return rv; +} + +static inline void set_timer_initalized(unsigned timer) +{ + once |= 1 << timer; +} + +static inline void set_timer_deinitalized(unsigned timer) +{ + once &= ~(1 << timer); +} + +static inline int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static inline int get_timers_firstchannels(unsigned timer) +{ + int channel = -1; + + if (validate_timer_index(timer) == 0) { + channel = timer_io_channels[io_timers[timer].first_channel_index].timer_channel; + } + + return channel; +} + +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + const io_timers_t *tmr = &io_timers[timer]; + + /* Gather the channel bits that belong to the timer */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + +static inline int is_channels_timer_uninitalized(unsigned channel) +{ + return is_timer_uninitalized(channels_timer(channel)); +} + +int io_timer_is_channel_free(unsigned channel) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { + rv = -EBUSY; + } + } + + return rv; +} + +int io_timer_validate_channel_index(unsigned channel) +{ + int rv = -EINVAL; + + if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { + + unsigned timer = timer_io_channels[channel].timer_index; + + /* test timer for validity */ + + if ((io_timers[timer].base != 0) && + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { + rv = 0; + } + } + + return rv; +} + +int io_timer_get_mode_channels(io_timer_channel_mode_t mode) +{ + if (mode < IOTimerChanModeSize) { + return channel_allocations[mode]; + } + + return 0; +} + +int io_timer_get_channel_mode(unsigned channel) +{ + io_timer_channel_allocation_t bit = 1 << channel; + + for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { + if (bit & channel_allocations[mode]) { + return mode; + } + } + + return -1; +} + +static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode, + io_timer_channel_mode_t new_mode) +{ + /* If caller mode is not based on current setting adjust it */ + + if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) { + mode = IOTimerChanMode_NotUsed; + } + + /* Remove old set of channels from original */ + + channel_allocations[mode] &= ~channels; + + /* Will this change ?*/ + + uint32_t before = channel_allocations[new_mode] & channels; + + /* add in the new set */ + + channel_allocations[new_mode] |= channels; + + /* Indicate a mode change */ + + return before ^ channels; +} + +static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = io_timer_is_channel_free(channel); + + if (rv == 0) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; + channel_allocations[mode] |= bit; + } + + return rv; +} + + +static inline int free_channel_resource(unsigned channel) +{ + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[mode] &= ~bit; + channel_allocations[IOTimerChanMode_NotUsed] |= bit; + } + + return mode; +} + +int io_timer_free_channel(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_set_enable(false, mode, 1 << channel); + free_channel_resource(channel); + + } + + return 0; +} + + +static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = -EINVAL; + + if (mode != IOTimerChanMode_NotUsed) { + rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + rv = allocate_channel_resource(channel, mode); + } + } + + return rv; +} + +static int timer_set_rate(unsigned timer, unsigned rate) +{ + /* configure the timer to update at the desired rate */ + //rARR(timer) = 1000000 / rate; + + /* generate an update event; reloads the counter and all registers */ + //rEGR(timer) = GTIM_EGR_UG; + + return 0; +} +static inline void io_timer_set_oneshot_mode(unsigned timer) +{ + /* Ideally, we would want per channel One pulse mode in HW + * Alas OPE stops the Timer not the channel + * todo:We can do this in an ISR later + * But since we do not have that + * We try to get the longest rate we can. + * On 16 bit timers this is 8.1 Ms. + * On 32 but timers this is 536870.912 + */ + +#if 0 + rARR(timer) = 0xffffffff; + rPSC(timer) = (io_timers[timer].clock_freq / BOARD_ONESHOT_FREQ) - 1; + rEGR(timer) = GTIM_EGR_UG; +#endif +} + +static inline void io_timer_set_PWM_mode(unsigned timer) +{ +// rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; +} + +void io_timer_trigger(void) +{ + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + + if (oneshots != 0) { + uint32_t action_cache[MAX_IO_TIMERS] = {0}; + int actions = 0; + + /* Pre-calculate the list of timers to Trigger */ + + for (int timer = 0; timer < MAX_IO_TIMERS; timer++) { + if (validate_timer_index(timer) == 0) { + int channels = get_timer_channels(timer); + + if (oneshots & channels) { + action_cache[actions++] = io_timers[timer].base; + } + } + } + + /* Now do them all wit the shortest delay in between */ + + irqstate_t flags = px4_enter_critical_section(); + + for (actions = 0; action_cache[actions] != 0 && actions < MAX_IO_TIMERS; actions++) { +//todo: _REG32(action_cache[actions], STM32_GTIM_EGR_OFFSET) |= GTIM_EGR_UG; + } + + px4_leave_critical_section(flags); + } +} + +int io_timer_init_timer(unsigned timer) +{ + /* Do this only once per timer */ + + int rv = is_timer_uninitalized(timer); + + if (rv == 0) { + + irqstate_t flags = px4_enter_critical_section(); + + set_timer_initalized(timer); + + /* enable the timer clock before we try to talk to it */ +#if 0 + modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCR1(timer) = 0; + rCCR2(timer) = 0; + rCCR3(timer) = 0; + rCCR4(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) { + + /* master output enable = on */ + + rBDTR(timer) = ATIM_BDTR_MOE; + } + + io_timer_set_PWM_mode(timer); + + + /* + * Note we do the Standard PWM Out init here + * default to updating at 50Hz + */ +#endif + timer_set_rate(timer, 50); + + /* + * Note that the timer is left disabled with IRQ subs installed + * and active but DEIR bits are not set. + */ +#if 0 + irq_attach(io_timers[timer].vectorno, io_timers[timer].handler); + + up_enable_irq(io_timers[timer].vectorno); +#endif + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_rate(unsigned timer, unsigned rate) +{ + int rv = EBUSY; + + /* Get the channel bits that belong to the timer */ + + uint32_t channels = get_timer_channels(timer); + + /* Check that all channels are either in PWM or Oneshot */ + + if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | + channel_allocations[IOTimerChanMode_OneShot] | + channel_allocations[IOTimerChanMode_NotUsed])) == + channels) { + + /* Change only a timer that is owned by pwm or one shot */ + + /* Request to use OneShot ?*/ + + if (rate == 0) { + + /* Request to use OneShot + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be Oneshot + */ + + /* Did the allocation change */ + if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { + io_timer_set_oneshot_mode(timer); + } + + } else { + + /* Request to use PWM + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be PWM + */ + + if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { + io_timer_set_PWM_mode(timer); + } + + timer_set_rate(timer, rate); + } + + rv = OK; + } + + return rv; +} + +int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context) +{ +#if 0 + uint32_t gpio = 0; + uint32_t clearbits = CCMR_C1_RESET; + uint32_t setbits = CCMR_C1_CAPTURE_INIT; + uint32_t ccer_setbits = CCER_C1_INIT; + uint32_t dier_setbits = GTIM_DIER_CC1IE; + + /* figure out the GPIO config first */ + + switch (mode) { + + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + ccer_setbits = 0; + dier_setbits = 0; + setbits = CCMR_C1_PWMOUT_INIT; + break; + + case IOTimerChanMode_PWMIn: + setbits = CCMR_C1_PWMIN_INIT; + gpio = timer_io_channels[channel].gpio_in; + break; + +#if defined(BOARD_HAS_CAPTURE) + + case IOTimerChanMode_Capture: + setbits = CCMR_C1_CAPTURE_INIT; + gpio = timer_io_channels[channel].gpio_in; + break; +#endif + + case IOTimerChanMode_NotUsed: + setbits = 0; + break; + + default: + return -EINVAL; + } + +#endif + int rv = allocate_channel(channel, mode); +#if 0 + /* Valid channel should now be reserved in new mode */ + + if (rv >= 0) { + + /* Blindly try to initialize the timer - it will only do it once */ + + io_timer_init_timer(channels_timer(channel)); + + irqstate_t flags = enter_critical_section(); + + irqstate_t flags = px4_enter_critical_section(); + + /* Set up IO */ + if (gpio) { + px4_arch_configgpio(gpio); + } + + + unsigned timer = channels_timer(channel); + + + /* configure the channel */ + + uint32_t shifts = timer_io_channels[channel].timer_channel - 1; + + /* Map shifts timer channel 1-4 to 0-3 */ + + uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t)); + uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t)); + + clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; + setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; + + uint16_t rvalue = REG(timer, ccmr_offset); + rvalue &= ~clearbits; + rvalue |= setbits; + REG(timer, ccmr_offset) = rvalue; + + /* + * The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode + * As soon as CCxS (in SSMRx becomes different from 00, the channel is configured + * in input and the TIMx_CCR1 register becomes read-only. + * so the next line does nothing in capture mode and initializes an PWM out to + * 0 + */ + + REG(timer, ccr_offset) = 0; + + /* on PWM Out ccer_setbits is 0 */ + + clearbits = (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS); + setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS); + rvalue = rCCER(timer); + rvalue &= ~clearbits; + rvalue |= setbits; + rCCER(timer) = rvalue; + +#if !defined(BOARD_HAS_CAPTURE) + UNUSED(dier_setbits); +#else + channel_handlers[channel].callback = channel_handler; + channel_handlers[channel].context = context; + rDIER(timer) |= dier_setbits << shifts; +#endif + px4_leave_critical_section(flags); + } + +#endif + return rv; +} + +int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks) +{ +#if 0 + struct action_cache_t { + uint32_t ccer_clearbits; + uint32_t ccer_setbits; + uint32_t dier_setbits; + uint32_t dier_clearbits; + uint32_t base; + uint32_t gpio[MAX_CHANNELS_PER_TIMER]; + } action_cache[MAX_IO_TIMERS]; + memset(action_cache, 0, sizeof(action_cache)); + + uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0; + uint32_t ccer_bit = state ? CCER_C1_INIT : 0; + + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + dier_bit = 0; + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + break; + + default: + return -EINVAL; + } + + /* Was the request for all channels in this mode ?*/ + + if (masks == IO_TIMER_ALL_MODES_CHANNELS) { + + /* Yes - we provide them */ + + masks = channel_allocations[mode]; + + } else { + + /* No - caller provided mask */ + + /* Only allow the channels in that mode to be affected */ + + masks &= channel_allocations[mode]; + + } + + /* Pre calculate all the changes */ + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1; + uint32_t timer = channels_timer(chan_index); + action_cache[timer].base = io_timers[timer].base; + action_cache[timer].ccer_clearbits |= CCER_C1_INIT << (shifts * CCER_C1_NUM_BITS); + action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS); + action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts; + action_cache[timer].dier_setbits |= dier_bit << shifts; + + if ((state && + (mode == IOTimerChanMode_PWMOut || + mode == IOTimerChanMode_OneShot || + mode == IOTimerChanMode_Trigger))) { + action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; + } + } + } + + irqstate_t flags = px4_enter_critical_section(); + + + for (unsigned actions = 0; actions < arraySize(action_cache); actions++) { + if (action_cache[actions].base != 0) { + uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET); + rvalue &= ~action_cache[actions].ccer_clearbits; + rvalue |= action_cache[actions].ccer_setbits; + _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue; + uint32_t after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); + + rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET); + rvalue &= ~action_cache[actions].dier_clearbits; + rvalue |= action_cache[actions].dier_setbits; + _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue; + + /* Any On ?*/ + + if (after != 0) { + + /* force an update to preload all registers */ + rEGR(actions) = GTIM_EGR_UG; + + for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) { + if (action_cache[actions].gpio[chan]) { + px4_arch_configgpio(action_cache[actions].gpio[chan]); + action_cache[actions].gpio[chan] = 0; + } + } + + /* arm requires the timer be enabled */ + rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + + rCR1(actions) = 0; + } + } + } + + leave_critical_section(flags); +#endif + return 0; +} + +int io_timer_set_ccr(unsigned channel, uint16_t value) +{ + int rv = io_timer_validate_channel_index(channel); + int mode = io_timer_get_channel_mode(channel); + + if (rv == 0) { + if ((mode != IOTimerChanMode_PWMOut) && + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Trigger)) { + + rv = -EIO; + + } else { + + /* configure the channel */ + +// REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value; + } + } + + return rv; +} + +uint16_t io_channel_get_ccr(unsigned channel) +{ + uint16_t value = 0; + + if (io_timer_validate_channel_index(channel) == 0) { + int mode = io_timer_get_channel_mode(channel); + + if ((mode == IOTimerChanMode_PWMOut) || + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { + value = 0; //REG(channels_timer(channel), timer_io_channels[channel].ccr_offset); + } + } + + return value; +} + +uint32_t io_timer_get_group(unsigned timer) +{ + return get_timer_channels(timer); + +} diff --git a/src/drivers/samv7/drv_io_timer.h b/src/drivers/samv7/drv_io_timer.h new file mode 100644 index 0000000000..77e7700604 --- /dev/null +++ b/src/drivers/samv7/drv_io_timer.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2017 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 drv_io_timer.h + * + * samv7-specific PWM output data. + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#define MAX_IO_TIMERS 4 +#define MAX_TIMER_IO_CHANNELS 8 + +#define MAX_LED_TIMERS 2 +#define MAX_TIMER_LED_CHANNELS 6 + +#define IO_TIMER_ALL_MODES_CHANNELS 0 + +typedef enum io_timer_channel_mode_t { + IOTimerChanMode_NotUsed = 0, + IOTimerChanMode_PWMOut = 1, + IOTimerChanMode_PWMIn = 2, + IOTimerChanMode_Capture = 3, + IOTimerChanMode_OneShot = 4, + IOTimerChanMode_Trigger = 5, + IOTimerChanModeSize +} io_timer_channel_mode_t; + +typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */ + +/* array of timers dedicated to PWM in and out and capture use + *** Note that the clock_freq is set to the source in the clock tree that + *** feeds this specific timer. This can differs by Timer! + *** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz + *** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz + *** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz + *** the resulting PSC will be one and the timer will count at it's clock frequency. + */ +typedef struct io_timers_t { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; + uint32_t vectorno; + uint32_t first_channel_index; + uint32_t last_channel_index; + xcpt_t handler; +} io_timers_t; + +/* array of channels in logical order */ +typedef struct timer_io_channels_t { + uint32_t gpio_out; + uint32_t gpio_in; + uint8_t timer_index; + uint8_t timer_channel; + uint16_t masks; + uint8_t ccr_offset; +} timer_io_channels_t; + + +typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt); + + +/* supplied by board-specific code */ +__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS]; +__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS]; + +__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; +__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; + +__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; +__EXPORT int io_timer_handler0(int irq, void *context, void *arg); +__EXPORT int io_timer_handler1(int irq, void *context, void *arg); +__EXPORT int io_timer_handler2(int irq, void *context, void *arg); +__EXPORT int io_timer_handler3(int irq, void *context, void *arg); + +__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context); + +__EXPORT int io_timer_init_timer(unsigned timer); + +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, + io_timer_channel_allocation_t masks); +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT uint16_t io_channel_get_ccr(unsigned channel); +__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); +__EXPORT uint32_t io_timer_get_group(unsigned timer); +__EXPORT int io_timer_validate_channel_index(unsigned channel); +__EXPORT int io_timer_is_channel_free(unsigned channel); +__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_get_channel_mode(unsigned channel); +__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); +__EXPORT extern void io_timer_trigger(void); + +__END_DECLS diff --git a/src/drivers/samv7/drv_pwm_servo.c b/src/drivers/samv7/drv_pwm_servo.c new file mode 100644 index 0000000000..28a6e02bce --- /dev/null +++ b/src/drivers/samv7/drv_pwm_servo.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with samv7 timers + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drv_io_timer.h" +#include "drv_pwm_servo.h" + +#include +#include +#include + + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_free_channel(channel); + current &= ~(1 << channel); + } + } + + // Now allocate the new set + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not PWM mode before + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + return OK; +} + +void up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) +{ + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) { + return -ERANGE; + } + + if (rate > 10000) { + return -ERANGE; + } + + if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) { + return ERROR; + } + + io_timer_set_rate(group, rate); + + return OK; +} + +int up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { + up_pwm_servo_set_rate_group_update(i, rate); + } + + return 0; +} + +void up_pwm_update(void) +{ + io_timer_trigger(); +} + +uint32_t up_pwm_servo_get_rate_group(unsigned group) +{ + return io_timer_get_group(group); +} + +void +up_pwm_servo_arm(bool armed) +{ + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/src/drivers/samv7/drv_pwm_servo.h b/src/drivers/samv7/drv_pwm_servo.h new file mode 100644 index 0000000000..e3477183d2 --- /dev/null +++ b/src/drivers/samv7/drv_pwm_servo.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 drv_pwm_servo.h + * + * stm32-specific PWM output data. + */ + +#pragma once + +#include diff --git a/src/drivers/samv7/drv_pwm_trigger.h b/src/drivers/samv7/drv_pwm_trigger.h new file mode 100644 index 0000000000..e27902f514 --- /dev/null +++ b/src/drivers/samv7/drv_pwm_trigger.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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 drv_pwm_trigger.h + * + * stm32-specific PWM output data. + */ + +#pragma once + +#include diff --git a/src/drivers/samv7/tone_alarm/CMakeLists.txt b/src/drivers/samv7/tone_alarm/CMakeLists.txt new file mode 100644 index 0000000000..3e9b5fd5c0 --- /dev/null +++ b/src/drivers/samv7/tone_alarm/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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 drivers__samv7__tone_alarm + MAIN tone_alarm + COMPILE_FLAGS + -Os + SRCS + tone_alarm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/samv7/tone_alarm/module.mk b/src/drivers/samv7/tone_alarm/module.mk new file mode 100644 index 0000000000..25a194ef61 --- /dev/null +++ b/src/drivers/samv7/tone_alarm/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Tone alarm driver +# + +MODULE_COMMAND = tone_alarm + +SRCS = tone_alarm.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/samv7/tone_alarm/tone_alarm.cpp b/src/drivers/samv7/tone_alarm/tone_alarm.cpp new file mode 100644 index 0000000000..afb470a5cd --- /dev/null +++ b/src/drivers/samv7/tone_alarm/tone_alarm.cpp @@ -0,0 +1,1031 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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. + * + ****************************************************************************/ + +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * + * The tone_alarm driver supports a set of predefined "alarm" + * tunes and one user-supplied tune. + * + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm tune, from 1 - . Selecting tune zero silences + * the alarm. + * + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_CHANNEL) && defined(HRT_TIMER_CHANNEL) +# if TONE_ALARM_CHANNEL == HRT_TIMER_CHANNEL +# error TONE_ALARM_CHANNEL and HRT_TIMER_CHANNEL must use different timers. +# endif +#endif + +/* Tone alarm configuration */ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/********************* TONE_ALARM_NOT_DONE ****************/ +/****** This code is not finished ***********/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +/**********************************************************/ +#ifdef TONE_ALARM_CHANNEL + +#if defined(TONE_ALARM_TIMER) +# error "TONE_ALARM_TIMER should not be defined, instead define TONE_ALARM_CHANNEL from 0-11" +#endif + +#if TONE_ALARM_CHANNEL == 0 || TONE_ALARM_CHANNEL == 1 || TONE_ALARM_CHANNEL == 2 +# define TONE_ALARM_TIMER 0 +#endif +#if TONE_ALARM_CHANNEL == 3 || TONE_ALARM_CHANNEL == 4 || TONE_ALARM_CHANNEL == 5 +# define TONE_ALARM_TIMER 1 +#endif +#if TONE_ALARM_CHANNEL == 6 || TONE_ALARM_CHANNEL == 7 || TONE_ALARM_CHANNEL == 8 +# define TONE_ALARM_TIMER 2 +#endif +#if TONE_ALARM_CHANNEL == 9 || TONE_ALARM_CHANNEL == 10 || TONE_ALARM_CHANNEL == 11 +# define TONE_ALARM_TIMER 3 +#endif + +/* HRT configuration */ +#if TONE_ALARM_TIMER == 0 +# define HRT_TIMER_BASE SAM_TC012_BASE +# if !defined(CONFIG_SAMV7_TC0) +# error "HRT_TIMER_CHANNEL 0-2 Require CONFIG_SAMV7_TC0=y" +# endif +#elif TONE_ALARM_TIMER == 1 +# define HRT_TIMER_BASE SAM_TC345_BASE +# if !defined(CONFIG_SAMV7_TC1) +# error "HRT_TIMER_CHANNEL 3-5 Require CONFIG_SAMV7_TC1=y" +# endif +#elif TONE_ALARM_TIMER == 2 +# define HRT_TIMER_BASE SAM_TC678_BASE +# if !defined(CONFIG_SAMV7_TC2) +# error "HRT_TIMER_CHANNEL 6-8 Require CONFIG_SAMV7_TC2=y" +# endif +#elif TONE_ALARM_TIMER == 3 +# define HRT_TIMER_BASE SAM_TC901_BASE +# if !defined(CONFIG_SAMV7_TC3) +# error "HRT_TIMER_CHANNEL 9-11 Require CONFIG_SAMV7_TC3=y" +# endif +#else +# error "HRT_TIMER_CHANNEL should be defined valid value are from 0-11" +# endif + +#define TONE_ALARM_CLOCK (BOARD_MCK_FREQUENCY/128) + + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(SAM_TC0_BASE + SAM_TC_CHAN_OFFSET(HRT_TIMER_CHANNEL) + _reg)) + +#define rCCR REG(SAM_TC_CCR_OFFSET) +#define rCMR REG(SAM_TC_CMR_OFFSET) +#define rSMMR REG(SAM_TC_SMMR_OFFSET) +#define rRAB REG(SAM_TC_RAB_OFFSET) +#define rCV REG(SAM_TC_CV_OFFSET) +#define rRA REG(SAM_TC_RA_OFFSET) +#define rRB REG(SAM_TC_RB_OFFSET) +#define rRC REG(SAM_TC_RC_OFFSET) +#define rSR REG(SAM_TC_SR_OFFSET) +#define rIER REG(SAM_TC_IER_OFFSET) +#define rIDR REG(SAM_TC_IDR_OFFSET) +#define rIMR REG(SAM_TC_IMR_OFFSET) +#define rEMR REG(SAM_TC_EMR_OFFSET) + +#define CBRK_BUZZER_KEY 782097 + +class ToneAlarm : public device::CDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) + { + return _tune_names[tune]; + } + + enum { + CBRK_OFF = 0, + CBRK_ON, + CBRK_UNINIT + }; + +private: + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; + static const uint8_t _note_tab[]; + + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + int _cbrk; //if true, no audio output + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // + static void next_trampoline(void *arg); + +}; + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + + +ToneAlarm::ToneAlarm() : + CDev("tone_alarm", TONEALARM0_DEVICE_PATH), + _default_tune_number(0), + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr), + _cbrk(CBRK_UNINIT) +{ + // enable debug() calls + //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } + + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); + return; + } + + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + + if (c == 0) { + goto tune_end; + } + + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + + if (_note_length < 1) { + goto tune_error; + } + + break; + + case 'O': // select octave + _octave = next_number(); + + if (_octave > 6) { + _octave = 6; + } + + break; + + case '<': // decrease octave + if (_octave > 0) { + _octave--; + } + + break; + + case '>': // increase octave + if (_octave < 6) { + _octave++; + } + + break; + + case 'M': // select inter-note gap + c = next_char(); + + if (c == 0) { + goto tune_error; + } + + _next++; + + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + + case 'L': + _note_mode = MODE_LEGATO; + break; + + case 'S': + _note_mode = MODE_STACCATO; + break; + + case 'F': + _repeat = false; + break; + + case 'B': + _repeat = true; + break; + + default: + goto tune_error; + } + + break; + + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; + } + + case 'N': // play an arbitrary note + note = next_number(); + + if (note > 84) { + goto tune_error; + } + + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + + break; + + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) { + note++; + } + + _next++; + break; + + case '-': // down a semitone + if (note > 1) { + note--; + } + + _next++; + break; + + default: + // 0 / no next char here is OK + break; + } + + // shorthand length notation + note_length = next_number(); + + if (note_length == 0) { + note_length = _note_length; + } + + break; + + default: + goto tune_error; + } + } + + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; + + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + syslog(LOG_ERR, "tune error\n"); + _repeat = false; // don't loop on error + + // stop (and potentially restart) the tune +tune_end: + stop_note(); + + if (_repeat) { + start_tune(_tune); + + } else { + _tune = nullptr; + _default_tune_number = 0; + } + + return; +} + +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; + } + + return toupper(*_next); +} + +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + + if (!isdigit(c)) { + return number; + } + + _next++; + number = (number * 10) + (c - '0'); + } +} + +unsigned +ToneAlarm::next_dots() +{ + unsigned dots = 0; + + while (next_char() == '.') { + _next++; + dots++; + } + + return dots; +} + +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + DEVICE_DEBUG("ioctl %i %u", cmd, arg); + +// irqstate_t flags = enter_critical_section(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + DEVICE_DEBUG("TONE_SET_ALARM %u", arg); + + if (arg < TONE_NUMBER_OF_TUNES) { + if (arg == TONE_STOP_TUNE) { + // stop the tune + _tune = nullptr; + _next = nullptr; + _repeat = false; + _default_tune_number = 0; + + } else { + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ + _default_tune_number = arg; + start_tune(_default_tunes[arg]); + } + } + + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; + } + +// leave_critical_section(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) { + result = CDev::ioctl(filp, cmd, arg); + } + + return result; +} + +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) +{ + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) { + return -EFBIG; + } + + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') { + return OK; + } + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + + if (_user_tune == nullptr) { + return -ENOMEM; + } + + // and play it + start_tune(_user_tune); + + return len; +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_tune(unsigned tune) +{ + exit(0); + + int fd, ret; + + fd = open(TONEALARM0_DEVICE_PATH, 0); + + if (fd < 0) { + err(1, TONEALARM0_DEVICE_PATH); + } + + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); + + if (ret != 0) { + err(1, "TONE_SET_ALARM"); + } + + exit(0); +} + +int +play_string(const char *str, bool free_buffer) +{ + int fd, ret; + + fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + + if (fd < 0) { + err(1, TONEALARM0_DEVICE_PATH); + } + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (free_buffer) { + free((void *)str); + } + + if (ret < 0) { + err(1, "play tune"); + } + + exit(0); +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned tune = 0; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) { + errx(1, "couldn't allocate the ToneAlarm driver"); + } + + if (g_dev->init() != OK) { + delete g_dev; + errx(1, "ToneAlarm init failed"); + } + } + + if (argc > 1) { + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) { + play_tune(TONE_STOP_TUNE); + } + + if (!strcmp(argv1, "stop")) { + play_tune(TONE_STOP_TUNE); + } + + if ((tune = strtol(argv1, nullptr, 10)) != 0) { + play_tune(tune); + } + + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv1)) { + play_tune(tune); + } + + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + + if (fd == nullptr) { + errx(1, "couldn't open '%s'", argv1); + } + + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + + if (buffer == nullptr) { + errx(1, "not enough memory memory"); + } + + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } + + /* if it looks like a PLAY string... */ + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); + } + } + + } + + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); +} +#endif /* TONE_ALARM_CHANNEL */