diff options
52 files changed, 1144 insertions, 69 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 833425aa6..a8ce3ea77 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -51,7 +51,7 @@ #include <time.h> #include <systemlib/err.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index c68a26df9..70b8ad6de 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -42,7 +42,7 @@ #include <fcntl.h> #include <unistd.h> #include <drivers/drv_gpio.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 7bb8490e5..3a432c3eb 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -59,7 +59,7 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/parameter_update.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 121b6d162..b53b675df 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -62,8 +62,8 @@ #include <v1.0/common/mavlink.h> #include <string.h> #include <arch/board/drv_led.h> -#include <arch/board/up_hrt.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> #include "state_machine_helper.h" #include "systemlib/systemlib.h" diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index b21f3858f..794fb679c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -45,7 +45,7 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <systemlib/systemlib.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index c554df9ae..8b22b1326 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -58,7 +58,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <drivers/device/spi.h> diff --git a/apps/drivers/boards/px4fmu/Makefile b/apps/drivers/boards/px4fmu/Makefile new file mode 100644 index 000000000..393e96e32 --- /dev/null +++ b/apps/drivers/boards/px4fmu/Makefile @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4FMU +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/nuttx/configs/px4fmu/src/up_hrt.c b/apps/drivers/boards/px4fmu/drv_hrt.c index c8ff1ce38..11b98fd1b 100644 --- a/nuttx/configs/px4fmu/src/up_hrt.c +++ b/apps/drivers/boards/px4fmu/drv_hrt.c @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file High-resolution timer callouts and timekeeping. + * @file drv_hrt.c + * + * High-resolution timer callouts and timekeeping. * * This can use any general or advanced STM32 timer. * @@ -58,7 +60,7 @@ #include <string.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "chip.h" #include "up_internal.h" @@ -320,16 +322,16 @@ static void hrt_call_invoke(void); /* decoded PPM buffer */ #define PPM_MAX_CHANNELS 12 -uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -unsigned ppm_decoded_channels; -uint64_t ppm_last_valid_decode = 0; +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT unsigned ppm_decoded_channels; +__EXPORT uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ -uint16_t ppm_edge_history[32]; +__EXPORT uint16_t ppm_edge_history[32]; unsigned ppm_edge_next; /* PPM pulse history */ -uint16_t ppm_pulse_history[32]; +__EXPORT uint16_t ppm_pulse_history[32]; unsigned ppm_pulse_next; static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 000000000..c35290589 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -0,0 +1,290 @@ +/**************************************************************************** + * + * 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_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() 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 initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/arch.h> +#include <nuttx/spi.h> +#include <nuttx/i2c.h> +#include <nuttx/mmcsd.h> +#include <nuttx/analog/adc.h> +#include <nuttx/arch.h> + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include <arch/board/up_cpuload.h> +#include <arch/board/up_adc.h> +#include <arch/board/board.h> +#include <arch/board/drv_led.h> +#include <arch/board/drv_eeprom.h> + +#include <drivers/drv_hrt.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_lowprintf(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_lowprintf +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int nsh_archinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi3; +static struct i2c_dev_s *i2c1; +static struct i2c_dev_s *i2c2; +static struct i2c_dev_s *i2c3; + +#include <math.h> + +#ifdef __cplusplus +int matherr(struct __exception *e) { + return 1; +} +#else +int matherr(struct exception *e) { + return 1; +} +#endif + +int nsh_archinitialize(void) +{ + int result; + + /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ + + /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ + + /* configure the high-resolution time/callout interface */ +#ifdef CONFIG_HRT_TIMER + hrt_init(); +#endif + + /* configure CPU load estimation */ + #ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); + #endif + + /* set up the serial DMA polling */ +#ifdef SERIAL_HAVE_DMA + { + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + } +#endif + + message("\r\n"); + + up_ledoff(LED_BLUE); + up_ledoff(LED_AMBER); + + up_ledon(LED_BLUE); + + /* Configure user-space led driver */ + px4fmu_led_init(); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + if (!spi1) + { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 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_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + /* initialize I2C2 bus */ + + i2c2 = up_i2cinitialize(2); + if (!i2c2) { + message("[boot] FAILED to initialize I2C bus 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* set I2C2 speed */ + I2C_SETFREQUENCY(i2c2, 400000); + + + i2c3 = up_i2cinitialize(3); + if (!i2c3) { + message("[boot] FAILED to initialize I2C bus 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* set I2C3 speed */ + I2C_SETFREQUENCY(i2c3, 400000); + + /* try to attach, don't fail if device is not responding */ + (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, + FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, + FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, + FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); + +#if defined(CONFIG_STM32_SPI3) + /* Get the SPI port */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + if (!spi3) + { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + if (result != OK) + { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); +#endif /* SPI3 */ + + /* initialize I2C1 bus */ + + i2c1 = up_i2cinitialize(1); + if (!i2c1) { + message("[boot] FAILED to initialize I2C bus 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* set I2C1 speed */ + I2C_SETFREQUENCY(i2c1, 400000); + + /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ + + /* Get board information if available */ + + /* Initialize the user GPIOs */ + px4fmu_gpio_init(); + +#ifdef CONFIG_ADC + int adc_state = adc_devinit(); + if (adc_state != OK) + { + /* Try again */ + adc_state = adc_devinit(); + if (adc_state != OK) + { + /* Give up */ + message("[boot] FAILED adc_devinit: %d\n", adc_state); + return -ENODEV; + } + } +#endif + + return OK; +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/apps/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 000000000..c58a8a5c4 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_internal.h @@ -0,0 +1,166 @@ +/**************************************************************************** + * + * 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_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> +#include <stdint.h> + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +#ifdef CONFIG_STM32_SPI2 +# error "SPI2 is not supported on this board" +#endif + +#if defined(CONFIG_STM32_CAN1) +# warning "CAN1 is not supported on this board" +#endif + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + +/* External interrupts */ +#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +// XXX MPU6000 DRDY? + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + +/* User GPIOs + * + * GPIO0-1 are the buffered high-power GPIOs. + * GPIO2-5 are the USART2 pins. + * GPIO6-7 are the CAN2 pins. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) +#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* PWM + * + * The PX4FMU has five PWM outputs, of which only the output on + * pin PC8 is fixed assigned to this function. The other four possible + * pwm sources are the TX, RX, RTS and CTS pins of USART2 + * + * Alternate function mapping: + * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 + */ + +#ifdef CONFIG_PWM +# if defined(CONFIG_STM32_TIM3_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 3 +# elif defined(CONFIG_STM32_TIM8_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 8 +# endif +#endif + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +/**************************************************************************************************** + * Name: px4fmu_gpio_init + * + * Description: + * Called to configure the PX4FMU user GPIOs + * + ****************************************************************************************************/ + +extern void px4fmu_gpio_init(void); + + +// XXX additional SPI chipselect functions required? + +#endif /* __ASSEMBLY__ */ diff --git a/nuttx/configs/px4fmu/include/up_hrt.h b/apps/drivers/drv_hrt.h index 6fd66ad74..a6d501f53 100644 --- a/nuttx/configs/px4fmu/include/up_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -31,12 +31,13 @@ * ****************************************************************************/ -/* - * High-resolution timer callouts and timekeeping. +/** + * @file drv_hrt.h + * + * High-resolution timer with callouts and timekeeping. */ -#ifndef UP_HRT_H_ -#define UP_HRT_H_ +#pragma once #include <sys/types.h> #include <stdbool.h> @@ -44,6 +45,8 @@ #include <time.h> #include <queue.h> +__BEGIN_DECLS + /* * Absolute time, in microsecond units. * @@ -76,17 +79,17 @@ typedef struct hrt_call { /* * Get absolute time. */ -extern hrt_abstime hrt_absolute_time(void); +__EXPORT extern hrt_abstime hrt_absolute_time(void); /* * Convert a timespec to absolute time. */ -extern hrt_abstime ts_to_abstime(struct timespec *ts); +__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); /* * Convert absolute time to a timespec. */ -extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); /* * Call callout(arg) after delay has elapsed. @@ -94,12 +97,12 @@ extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); * If callout is NULL, this can be used to implement a timeout by testing the call * with hrt_called(). */ -extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg); +__EXPORT extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg); /* * Call callout(arg) at absolute time calltime. */ -extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg); +__EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg); /* * Call callout(arg) after delay, and then after every interval. @@ -107,7 +110,7 @@ extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callou * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may * jitter but should not drift. */ -extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); +__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); /* * If this returns true, the entry has been invoked and removed from the callout list, @@ -115,16 +118,16 @@ extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstim * * Always returns false for repeating callouts. */ -extern bool hrt_called(struct hrt_call *entry); +__EXPORT extern bool hrt_called(struct hrt_call *entry); /* * Remove the entry from the callout list. */ -extern void hrt_cancel(struct hrt_call *entry); +__EXPORT extern void hrt_cancel(struct hrt_call *entry); /* * Initialise the HRT. */ -extern void hrt_init(void); +__EXPORT extern void hrt_init(void); -#endif /* UP_HRT_H_ */ +__END_DECLS diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 8e78825c3..bfe79302d 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -58,7 +58,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -115,6 +115,10 @@ #endif static const int ERROR = -1; +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + class HMC5883 : public device::I2C { public: diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index e9b60b01c..4606cd654 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -57,7 +57,7 @@ #include <nuttx/arch.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <drivers/device/spi.h> diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7b8a84f7e..18b139b32 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -61,7 +61,7 @@ #include <nuttx/clock.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/device/spi.h> #include <drivers/drv_accel.h> diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 4df9de94c..893ef6c37 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -57,7 +57,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -70,6 +70,10 @@ #endif static const int ERROR = -1; +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + /** * Calibration PROM as reported by the device. */ diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index bfde83cde..00f225dd5 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -71,7 +71,7 @@ #include <unistd.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/stm32/chip.h> #include <up_internal.h> diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 8629c2f11..e04033481 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -48,7 +48,7 @@ #include <math.h> #include <poll.h> #include <time.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 2e90f50b1..374280d28 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -43,7 +43,7 @@ #include <pthread.h> #include <poll.h> #include <fcntl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> #include <mavlink/mavlink_log.h> diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index d1c9d364b..577a3a01c 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -44,7 +44,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> #include <mavlink/mavlink_log.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2 diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index b1655afd7..8a7d86b4e 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -40,7 +40,7 @@ #include "gps.h" #include <sys/prctl.h> #include <poll.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <string.h> #include <stdbool.h> diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 235f5f8f3..7d8232b3a 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -50,7 +50,7 @@ #include <string.h> #include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <time.h> #include <float.h> #include <unistd.h> diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 550746794..cfff0f469 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -49,7 +49,7 @@ #include <string.h> #include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <time.h> #include <float.h> #include <unistd.h> diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c index d2be9a88d..50282a9e3 100644 --- a/apps/mavlink/missionlib.c +++ b/apps/mavlink/missionlib.c @@ -49,7 +49,7 @@ #include <string.h> #include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <time.h> #include <float.h> #include <unistd.h> diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 0b073cc65..90b0073cf 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -46,7 +46,7 @@ #include <string.h> #include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <time.h> #include <float.h> #include <unistd.h> diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index dfb778fd0..0cdb53554 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -54,7 +54,7 @@ #include <math.h> #include <poll.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <drivers/drv_gyro.h> #include <uORB/topics/vehicle_status.h> diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 8c0e10fc3..eb94cca8a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -52,7 +52,7 @@ #include <math.h> #include <systemlib/pid/pid.h> #include <systemlib/param/param.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 42aa0ac63..a5bff97e6 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -53,7 +53,7 @@ #include <systemlib/pid/pid.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7cf687306..474ced731 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -48,7 +48,7 @@ #include <termios.h> #include <time.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude.h> diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 45267f315..60737a654 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -46,7 +46,7 @@ #include <sys/time.h> #include <stdbool.h> #include <fcntl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <string.h> #include <poll.h> #include <uORB/uORB.h> diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c index 8c6951b63..f364ea080 100644 --- a/apps/px4/tests/test_hrt.c +++ b/apps/px4/tests/test_hrt.c @@ -50,7 +50,7 @@ #include <unistd.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> #include <nuttx/spi.h> diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c index c128c73a3..25bf02c31 100644 --- a/apps/px4/tests/test_time.c +++ b/apps/px4/tests/test_time.c @@ -53,7 +53,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c index de1249b8c..fc5b03036 100644 --- a/apps/px4/tests/test_uart_console.c +++ b/apps/px4/tests/test_uart_console.c @@ -56,7 +56,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c index 83d205440..a88e617d9 100644 --- a/apps/px4/tests/test_uart_send.c +++ b/apps/px4/tests/test_uart_send.c @@ -56,7 +56,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/tests_file.c b/apps/px4/tests/tests_file.c index 2cff622f7..697410cee 100644 --- a/apps/px4/tests/tests_file.c +++ b/apps/px4/tests/tests_file.c @@ -44,7 +44,7 @@ #include <systemlib/perf_counter.h> #include <string.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "tests.h" diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 19802bf4f..f9106d830 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -48,7 +48,7 @@ #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 9dc1fdcba..a91aad3ca 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -47,7 +47,7 @@ #include <arch/board/drv_ppm_input.h> #include <arch/board/drv_pwm_servo.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 30a62fa65..a67db9a7e 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -50,7 +50,7 @@ #include <arch/board/up_boardinitialize.h> #include <arch/board/drv_gpio.h> #include <arch/board/drv_ppm_input.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 895c33806..6cb85798f 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -49,7 +49,7 @@ #include <arch/board/up_boardinitialize.h> #include <arch/board/drv_gpio.h> #include <arch/board/drv_ppm_input.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 36225386c..312768095 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -51,7 +51,7 @@ #include <string.h> #include <systemlib/err.h> #include <unistd.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 3e204662a..6b7ca658c 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -52,7 +52,7 @@ #include <errno.h> #include <math.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index 02c1bb133..7a1faa618 100644 --- a/apps/systemcmds/led/led.c +++ b/apps/systemcmds/led/led.c @@ -50,7 +50,7 @@ #include <termios.h> #include <time.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/board/drv_led.h> #include <systemlib/err.h> diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index f4e260211..b373d9ae7 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -47,7 +47,7 @@ #include <poll.h> #include <arch/board/up_cpuload.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /** * Start the top application. diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index b49a3c54a..4bc7033e7 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -41,8 +41,13 @@ CSRCS = err.c \ param/param.c \ bson/tinybson.c \ conversions.c \ + cpuload.c \ getopt_long.c +# ppm_decode.c \ + + + # # XXX this really should be a CONFIG_* test # diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c new file mode 100644 index 000000000..9e6df3fac --- /dev/null +++ b/apps/systemlib/cpuload.c @@ -0,0 +1,180 @@ +/**************************************************************************** + * configs/px4fmu/src/up_leds.c + * arch/arm/src/board/up_leds.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <sys/time.h> +#include <sched.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include "cpuload.h" + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +/**************************************************************************** + * Definitions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT void sched_note_start(FAR _TCB *tcb); +__EXPORT void sched_note_stop(FAR _TCB *tcb); +__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb); + +/**************************************************************************** + * Name: + ****************************************************************************/ + +__EXPORT struct system_load_s system_load; + +extern FAR _TCB *sched_gettcb(pid_t pid); + +void cpuload_initialize_once() +{ +// if (!system_load.initialized) +// { + system_load.start_time = hrt_absolute_time(); + int i; + for (i = 0; i < CONFIG_MAX_TASKS; i++) + { + system_load.tasks[i].valid = false; + } + system_load.total_count = 0; + + uint64_t now = hrt_absolute_time(); + + /* initialize idle thread statically */ + system_load.tasks[0].start_time = now; + system_load.tasks[0].total_runtime = 0; + system_load.tasks[0].curr_start_time = 0; + system_load.tasks[0].tcb = sched_gettcb(0); + system_load.tasks[0].valid = true; + system_load.total_count++; + + /* initialize init thread statically */ + system_load.tasks[1].start_time = now; + system_load.tasks[1].total_runtime = 0; + system_load.tasks[1].curr_start_time = 0; + system_load.tasks[1].tcb = sched_gettcb(1); + system_load.tasks[1].valid = true; + /* count init thread */ + system_load.total_count++; + // } +} + +void sched_note_start(FAR _TCB *tcb ) +{ + /* search first free slot */ + int i; + for (i = 1; i < CONFIG_MAX_TASKS; i++) + { + if (!system_load.tasks[i].valid) + { + /* slot is available */ + system_load.tasks[i].start_time = hrt_absolute_time(); + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = tcb; + system_load.tasks[i].valid = true; + system_load.total_count++; + break; + } + } +} + +void sched_note_stop(FAR _TCB *tcb ) +{ + int i; + for (i = 1; i < CONFIG_MAX_TASKS; i++) + { + if (system_load.tasks[i].tcb->pid == tcb->pid) + { + /* mark slot as fee */ + system_load.tasks[i].valid = false; + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = NULL; + system_load.total_count--; + break; + } + } +} + +void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) +{ + uint64_t new_time = hrt_absolute_time(); + + /* Kind of inefficient: find both tasks and update times */ + uint8_t both_found = 0; + for (int i = 0; i < CONFIG_MAX_TASKS; i++) + { + /* Task ending its current scheduling run */ + if (system_load.tasks[i].tcb->pid == pFromTcb->pid) + { + //if (system_load.tasks[i].curr_start_time != 0) + { + system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; + } + both_found++; + } + else if (system_load.tasks[i].tcb->pid == pToTcb->pid) + { + system_load.tasks[i].curr_start_time = new_time; + both_found++; + } + + /* Do only iterate as long as needed */ + if (both_found == 2) + { + break; + } + } +} + +#endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/apps/systemlib/cpuload.h b/apps/systemlib/cpuload.h new file mode 100644 index 000000000..a97047ea8 --- /dev/null +++ b/apps/systemlib/cpuload.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +__BEGIN_DECLS + +#include <nuttx/sched.h> + +struct system_load_taskinfo_s { + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct _TCB *tcb; ///< + bool valid; ///< Task is currently active / valid +}; + +struct system_load_s { + uint64_t start_time; ///< Global start time of measurements + struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; + uint8_t initialized; + int total_count; + int running_count; + int sleeping_count; +}; + +__EXPORT extern struct system_load_s system_load; + +__EXPORT void cpuload_initialize_once(void); + +#endif diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 9e886ea65..6dacb1635 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -50,7 +50,7 @@ #include <sys/stat.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "systemlib/param/param.h" #include "systemlib/uthash/utarray.h" diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c index e25e548f0..e6c49d432 100644 --- a/apps/systemlib/perf_counter.c +++ b/apps/systemlib/perf_counter.c @@ -40,7 +40,7 @@ #include <stdlib.h> #include <stdio.h> #include <sys/queue.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "perf_counter.h" diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c new file mode 100644 index 000000000..dd6d43a77 --- /dev/null +++ b/apps/systemlib/ppm_decode.c @@ -0,0 +1,242 @@ +/**************************************************************************** + * + * 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 ppm_decode.c + * + * PPM input decoder. + */ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "ppm_decode.h" + +/* + * PPM decoder tuning parameters. + * + * The PPM decoder works as follows. + * + * Initially, the decoder waits in the UNSYNCH state for two edges + * separated by PPM_MIN_START. Once the second edge is detected, + * the decoder moves to the ARM state. + * + * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the + * timing mark for the first channel. If this is detected, it moves to + * the INACTIVE state. + * + * The INACTIVE phase waits for and discards the next edge, as it is not + * significant. Once the edge is detected, it moves to the ACTIVE stae. + * + * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when + * received calculates the time from the previous mark and records + * this time as the value for the next channel. + * + * If at any time waiting for an edge, the delay from the previous edge + * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded + * values are advertised to clients. + */ +#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a 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 2500 /* shortest valid start gap */ + +/* Input timeout - after this interval we assume signal is lost */ +#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */ + +/* Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 4 +#define PPM_MAX_CHANNELS 12 + +/* + * Public decoder state + */ +uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +unsigned ppm_decoded_channels; +hrt_abstime ppm_last_valid_decode; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +static struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + unsigned count_max; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + + +void +ppm_input_init(unsigned count_max) +{ + ppm_decoded_channels = 0; + ppm_last_valid_decode = 0; + + memset(&ppm, 0, sizeof(ppm)); + ppm.count_max = count_max; +} + +void +ppm_input_decode(bool reset, unsigned count) +{ + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (reset) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + if (count < ppm.last_edge) + width += ppm.count_max; /* handle wrapped count */ + ppm.last_edge = count; + + /* + * If this looks like a start pulse, then push the last set of values + * and reset the state machine. + * + * Note that this is not a "high performance" design; it implies a whole + * frame of latency between the pulses being received and their being + * considered valid. + */ + 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; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + + /* note that we don't bother looking at the timing of this edge */ + + return; + + case ACTIVE: + /* we expect a well-formed pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + /* 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; + 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; +} + diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h new file mode 100644 index 000000000..681cbe830 --- /dev/null +++ b/apps/systemlib/ppm_decode.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 ppm_decode.h + * + * PPM input decoder. + */ + +#pragma once + +#include <drivers/drv_hrt.h> + +/** + * Maximum number of channels that we will decode. + */ +#define PPM_MAX_CHANNELS 12 + +__BEGIN_DECLS + +/* + * PPM decoder state + */ +__EXPORT extern uint16_t ppm_buffer[]; /**< decoded PPM channel values */ +__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ +__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ + +/** + * Initialise the PPM input decoder. + * + * @param count_max The maximum value of the counter passed to + * ppm_input_decode, used to determine how to + * handle counter wrap. + */ +__EXPORT void ppm_input_init(unsigned count_max); + +/** + * Inform the decoder of an edge on the PPM input. + * + * This function can be registered with the HRT as the PPM edge handler. + * + * @param reset If set, the edge detector has missed one or + * more edges and the decoder needs to be reset. + * @param count A microsecond timestamp corresponding to the + * edge, in the range 0-count_max. This value + * is expected to wrap. + */ +__EXPORT void ppm_input_decode(bool reset, unsigned count); + +__END_DECLS
\ No newline at end of file diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 72a60f871..532e54b8e 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -56,7 +56,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_orb_dev.h> diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index a1228bba0..b587b3386 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -87,6 +87,7 @@ CONFIGURED_APPS += px4/ground_estimator #CONFIGURED_APPS += tools/i2c_dev # Communication and Drivers +CONFIGURED_APPS += drivers/boards/px4fmu CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/hmc5883 diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 338f364fc..0768a9d60 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -40,15 +40,10 @@ CFLAGS += -I$(TOPDIR)/sched ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) -CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \ +CSRCS = up_boot.c up_leds.c up_spi.c \ drv_gpio.c \ drv_led.c drv_eeprom.c \ - up_pwm_servo.c up_usbdev.c \ - up_cpuload.c - -ifeq ($(CONFIG_NSH_ARCHINIT),y) -CSRCS += up_nsh.c -endif + up_pwm_servo.c up_usbdev.c ifeq ($(CONFIG_ADC),y) CSRCS += up_adc.c |