diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-24 10:39:27 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-24 10:39:27 +0200 |
commit | 4d03d020af38c5c11f13f4ea60152683d596412d (patch) | |
tree | ee3cb50981e92c3174021f259bddb2c168d58a36 /apps | |
parent | 18c009d2c1585e05e2cee456a8cf91c517e006d2 (diff) | |
parent | 1065118ebc963dbed75da5c7093b1863f0e099a6 (diff) | |
download | px4-firmware-4d03d020af38c5c11f13f4ea60152683d596412d.tar.gz px4-firmware-4d03d020af38c5c11f13f4ea60152683d596412d.tar.bz2 px4-firmware-4d03d020af38c5c11f13f4ea60152683d596412d.zip |
Merge branch 'master' of github.com:PX4/Firmware into gps
Diffstat (limited to 'apps')
89 files changed, 4026 insertions, 579 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..a7550b54b 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" @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -#include <arch/board/up_cpuload.h> +#include <systemlib/cpuload.h> extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ 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..bc4d4b3bf 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> @@ -289,6 +289,7 @@ BMA180::init() _num_reports = 2; _oldest_report = _next_report = 0; _reports = new struct accel_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -321,13 +322,16 @@ BMA180::init() modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); if (set_range(4)) warnx("Failed setting range"); + if (set_lowpass(75)) warnx("Failed setting lowpass"); if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { ret = OK; + } else { ret = ERROR; } + out: return ret; } @@ -441,6 +445,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { @@ -468,7 +473,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -488,12 +493,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -549,24 +554,30 @@ BMA180::set_range(unsigned max_g) if (max_g == 0) max_g = 16; + if (max_g > 16) return -ERANGE; if (max_g <= 2) { _current_range = 2; rangebits = OFFSET_LSB1_RANGE_2G; + } else if (max_g <= 3) { _current_range = 3; rangebits = OFFSET_LSB1_RANGE_3G; + } else if (max_g <= 4) { _current_range = 4; rangebits = OFFSET_LSB1_RANGE_4G; + } else if (max_g <= 8) { _current_range = 8; rangebits = OFFSET_LSB1_RANGE_8G; + } else if (max_g <= 16) { _current_range = 16; rangebits = OFFSET_LSB1_RANGE_16G; + } else { return -EINVAL; } @@ -586,7 +597,7 @@ BMA180::set_range(unsigned max_g) /* check if wanted value is now in register */ return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) == - (OFFSET_LSB1_RANGE_MASK & rangebits)); + (OFFSET_LSB1_RANGE_MASK & rangebits)); } int @@ -633,7 +644,7 @@ BMA180::set_lowpass(unsigned frequency) /* check if wanted value is now in register */ return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == - (BW_TCS_BW_MASK & bwbits)); + (BW_TCS_BW_MASK & bwbits)); } void @@ -703,9 +714,9 @@ BMA180::measure() * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x; - report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y; - report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z; + report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x; + report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y; + report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z; /* discard two non-value bits in the 16 bit measurement */ report->x_raw = (report->x_raw >> 2); @@ -781,17 +792,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -809,16 +824,18 @@ test() /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) - err(1, "%s open failed (try 'bma180 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + err(1, "%s open failed (try 'bma180 start' if the driver is not running)", + ACCEL_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -831,7 +848,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* XXX add poll-rate tests here too */ @@ -846,10 +863,13 @@ void reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); 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/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c new file mode 100644 index 000000000..987894163 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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_adc.c + * + * Board-specific ADC functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <errno.h> +#include <debug.h> +#include <stdio.h> + +#include <nuttx/analog/adc.h> +#include <arch/board/board.h> + +#include "chip.h" +#include "up_arch.h" + +#include "stm32_adc.h" +#include "px4fmu_internal.h" + +#define ADC3_NCHANNELS 4 + +/************************************************************************************ + * Private Data + ************************************************************************************/ +/* The PX4FMU board has four ADC channels: ADC323 IN10-13 + */ + +/* Identifying number of each ADC channel: Variable Resistor. */ + +#ifdef CONFIG_STM32_ADC3 +static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards + +/* Configurations of pins used byte each ADC channels */ +static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: adc_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/adc. + * + ************************************************************************************/ + +int adc_devinit(void) +{ + static bool initialized = false; + struct adc_dev_s *adc[ADC3_NCHANNELS]; + int ret; + int i; + + /* Check if we have already initialized */ + + if (!initialized) { + char name[11]; + + for (i = 0; i < ADC3_NCHANNELS; i++) { + stm32_configgpio(g_pinlist[i]); + } + + for (i = 0; i < 1; i++) { + /* Configure the pins as analog inputs for the selected channels */ + //stm32_configgpio(g_pinlist[i]); + + /* Call stm32_adcinitialize() to get an instance of the ADC interface */ + //multiple channels only supported with dma! + adc[i] = stm32_adcinitialize(3, (g_chanlist), 4); + + if (adc == NULL) { + adbg("ERROR: Failed to get ADC interface\n"); + return -ENODEV; + } + + + /* Register the ADC driver at "/dev/adc0" */ + sprintf(name, "/dev/adc%d", i); + ret = adc_register(name, adc[i]); + + if (ret < 0) { + adbg("adc_register failed for adc %s: %d\n", name, ret); + return ret; + } + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_can.c b/apps/drivers/boards/px4fmu/px4fmu_can.c new file mode 100644 index 000000000..0d0b5fcd3 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_can.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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_can.c + * + * Board-specific CAN functions. + */ + + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <errno.h> +#include <debug.h> + +#include <nuttx/can.h> +#include <arch/board/board.h> + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#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 STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(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; +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 000000000..e5ded7328 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * 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/board.h> +#include <arch/board/drv_led.h> +#include <arch/board/drv_eeprom.h> + +#include <drivers/drv_hrt.h> + +#include <systemlib/cpuload.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 + ****************************************************************************/ + +extern int adc_devinit(void); + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * 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 +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT 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/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c new file mode 100644 index 000000000..d1ff8c112 --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include <stdint.h> + +#include <drivers/stm32/drv_pwm_servo.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c new file mode 100644 index 000000000..70245a3ec --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_spi.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <nuttx/spi.h> +#include <arch/board/board.h> + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL); + stm32_configgpio(GPIO_SPI_CS_MPU); + stm32_configgpio(GPIO_SPI_CS_SDCARD); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); + break; + + case PX4_SPIDEV_ACCEL: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0); +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ + return SPI_STATUS_PRESENT; +} + diff --git a/apps/drivers/boards/px4fmu/px4fmu_usb.c b/apps/drivers/boards/px4fmu/px4fmu_usb.c new file mode 100644 index 000000000..b0b669fbe --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <nuttx/usb/usbdev.h> +#include <nuttx/usb/usbdev_trace.h> + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 9af678465..01692c315 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -54,7 +54,7 @@ /** * Namespace encapsulating all device framework classes, functions and data. */ -namespace device __EXPORT +namespace device __EXPORT { /** @@ -276,14 +276,14 @@ public: */ virtual int poll(struct file *filp, struct pollfd *fds, bool setup); - /** - * Test whether the device is currently open. - * - * This can be used to avoid tearing down a device that is still active. - * - * @return True if the device is currently open. - */ - bool is_open() { return _open_count > 0; } + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } protected: /** diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c4b2aa944..4b832b548 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -121,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len unsigned tries = 0; do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; @@ -144,7 +144,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (msgs == 0) return -EINVAL; - /* + /* * I2C architecture means there is an unavoidable race here * if there are any devices on the bus with a different frequency * preference. Really, this is pointless. @@ -154,7 +154,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (ret == OK) break; - + // reset the I2C bus to unwedge on error up_i2creset(_dev); diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 7c5a14d6b..eb1b6cb05 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -42,7 +42,7 @@ #include <nuttx/i2c.h> -namespace device __EXPORT +namespace device __EXPORT { /** diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index a1761b769..528333e04 100644 --- a/apps/drivers/device/spi.cpp +++ b/apps/drivers/device/spi.cpp @@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* do common setup */ if (!up_interrupt_context()) SPI_LOCK(_dev, true); + SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); SPI_SETBITS(_dev, 8); @@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); + if (!up_interrupt_context()) SPI_LOCK(_dev, false); diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h index b2a111562..e8c8e2c5e 100644 --- a/apps/drivers/device/spi.h +++ b/apps/drivers/device/spi.h @@ -84,7 +84,7 @@ protected: * If called from interrupt context, this interface does not lock * the bus and may interfere with non-interrupt-context callers. * - * Clients in a mixed interrupt/non-interrupt configuration must + * Clients in a mixed interrupt/non-interrupt configuration must * ensure appropriate interlocking. * * At least one of send or recv must be non-null. diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 6d0c8c545..3834795e0 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -83,7 +83,7 @@ ORB_DECLARE(sensor_accel); /* * ioctl() definitions * - * Accelerometer drivers also implement the generic sensor driver + * Accelerometer drivers also implement the generic sensor driver * interfaces from drv_sensor.h */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 5c646f243..0dbf05c5b 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -58,7 +58,7 @@ struct gyro_report { float temperature; /**< temperature in degrees celcius */ float range_rad_s; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h new file mode 100644 index 000000000..3b493a81a --- /dev/null +++ b/apps/drivers/drv_hrt.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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_hrt.h + * + * High-resolution timer with callouts and timekeeping. + */ + +#pragma once + +#include <sys/types.h> +#include <stdbool.h> + +#include <time.h> +#include <queue.h> + +__BEGIN_DECLS + +/* + * Absolute time, in microsecond units. + * + * Absolute time is measured from some arbitrary epoch shortly after + * system startup. It should never wrap or go backwards. + */ +typedef uint64_t hrt_abstime; + +/* + * Callout function type. + * + * Note that callouts run in the timer interrupt context, so + * they are serialised with respect to each other, and must not + * block. + */ +typedef void (* hrt_callout)(void *arg); + +/* + * Callout record. + */ +typedef struct hrt_call { + struct sq_entry_s link; + + hrt_abstime deadline; + hrt_abstime period; + hrt_callout callout; + void *arg; +} *hrt_call_t; + +/* + * Get absolute time. + */ +__EXPORT extern hrt_abstime hrt_absolute_time(void); + +/* + * Convert a timespec to absolute time. + */ +__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); + +/* + * Convert absolute time to a timespec. + */ +__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); + +/* + * Call callout(arg) after delay has elapsed. + * + * If callout is NULL, this can be used to implement a timeout by testing the call + * with hrt_called(). + */ +__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. + */ +__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. + * + * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may + * jitter but should not drift. + */ +__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, + * or it has never been entered. + * + * Always returns false for repeating callouts. + */ +__EXPORT extern bool hrt_called(struct hrt_call *entry); + +/* + * Remove the entry from the callout list. + */ +__EXPORT extern void hrt_cancel(struct hrt_call *entry); + +/* + * Initialise the HRT. + */ +__EXPORT extern void hrt_init(void); + +__END_DECLS diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 7ba9dd695..114bcb646 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -59,7 +59,7 @@ struct mag_report { float z; float range_ga; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 340175a4b..97033f2cd 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -41,14 +41,15 @@ * channel. */ -#ifndef _DRV_PWM_OUTPUT_H -#define _DRV_PWM_OUTPUT_H +#pragma once #include <stdint.h> #include <sys/ioctl.h> #include "drv_orb_dev.h" +__BEGIN_DECLS + /** * Path for the default PWM output device. * @@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) -#endif /* _DRV_PWM_OUTPUT_H */ +/* + * Low-level PWM output interface. + * + * This is the low-level API to the platform-specific PWM driver. + */ + +/** + * Intialise the PWM servo outputs using the specified configuration. + * + * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. + * This allows some of the channels to remain configured + * as GPIOs or as another function. + * @return OK on success. + */ +__EXPORT extern int up_pwm_servo_init(uint32_t channel_mask); + +/** + * De-initialise the PWM servo outputs. + */ +__EXPORT extern void up_pwm_servo_deinit(void); + +/** + * Arm or disarm servo outputs. + * + * When disarmed, servos output no pulse. + * + * @bug This function should, but does not, guarantee that any pulse + * currently in progress is cleanly completed. + * + * @param armed If true, outputs are armed; if false they + * are disarmed. + */ +__EXPORT extern void up_pwm_servo_arm(bool armed); + +/** + * Set the servo update rate + * + * @param rate The update rate in Hz to set. + * @return OK on success, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate(unsigned rate); + +/** + * Set the current output value for a channel. + * + * @param channel The channel to set. + * @param value The output pulse width in microseconds. + */ +__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); + +/** + * Get the current output value for a channel. + * + * @param channel The channel to read. + * @return The output pulse width in microseconds, or zero if + * outputs are not armed or not configured. + */ +__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); + +__END_DECLS diff --git a/apps/drivers/drv_sensor.h b/apps/drivers/drv_sensor.h index 325bd42bf..3a89cab9d 100644 --- a/apps/drivers/drv_sensor.h +++ b/apps/drivers/drv_sensor.h @@ -52,7 +52,7 @@ #define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) /** - * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE + * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE * constants */ #define SENSORIOCSPOLLRATE _SENSORIOC(0) @@ -68,8 +68,8 @@ #define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */ #define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */ -/** - * Set the internal queue depth to (arg) entries, must be at least 1 +/** + * Set the internal queue depth to (arg) entries, must be at least 1 * * This sets the upper bound on the number of readings that can be * read from the driver. diff --git a/apps/drivers/drv_tone_alarm.h b/apps/drivers/drv_tone_alarm.h index 27b146de9..0c6afc64c 100644 --- a/apps/drivers/drv_tone_alarm.h +++ b/apps/drivers/drv_tone_alarm.h @@ -34,8 +34,8 @@ /* * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 8e78825c3..7943012cc 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: @@ -328,13 +332,16 @@ HMC5883::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct mag_report[_num_reports]; + if (_reports == nullptr) goto out; + _oldest_report = _next_report = 0; /* get a publish handle on the mag topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -354,30 +361,37 @@ int HMC5883::set_range(unsigned range) range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; + } else if (range <= 1) { range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; + } else if (range <= 2) { range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; + } else if (range <= 3) { range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; + } else if (range <= 4) { range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; + } else if (range <= 4.7f) { range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; + } else if (range <= 5.6f) { range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; + } else { range_bits = 0x07; _range_scale = 1.0f / 230.0f; @@ -409,10 +423,12 @@ HMC5883::probe() uint8_t data[3] = {0, 0, 0}; _retries = 10; + if (read_reg(ADDR_ID_A, data[0]) || read_reg(ADDR_ID_B, data[1]) || read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); + _retries = 1; if ((data[0] != ID_A_WHO_AM_I) || @@ -548,6 +564,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { @@ -662,7 +679,7 @@ HMC5883::cycle() if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -680,7 +697,7 @@ HMC5883::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -846,7 +863,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("starting mag scale calibration"); /* do a simple demand read */ - sz = read(filp, (char*)&report, sizeof(report)); + sz = read(filp, (char *)&report, sizeof(report)); + if (sz != sizeof(report)) { warn("immediate read failed"); ret = 1; @@ -916,6 +934,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); goto out; + } else { avg_excited[0] += report.x; avg_excited[1] += report.y; @@ -942,7 +961,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = fabsf(1.08f / avg_excited[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - + /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { @@ -967,12 +986,15 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = OK; - out: - if (ret == OK) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration failed."); - } +out: + + if (ret == OK) { + warnx("mag scale calibration successfully finished."); + + } else { + warnx("mag scale calibration failed."); + } + return ret; } @@ -982,16 +1004,22 @@ int HMC5883::set_excitement(unsigned enable) /* arm the excitement strap */ uint8_t conf_reg; ret = read_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) perf_count(_comms_errors); + if (((int)enable) < 0) { conf_reg |= 0x01; + } else if (enable > 0) { conf_reg |= 0x02; + } else { conf_reg &= ~0x03; } + ret = write_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) perf_count(_comms_errors); @@ -1083,17 +1111,22 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -1110,11 +1143,13 @@ test() int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) err(1, "immediate read failed"); @@ -1163,7 +1198,7 @@ test() * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor - * + * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor @@ -1203,6 +1238,7 @@ int calibrate() int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); @@ -1214,6 +1250,7 @@ int calibrate() if (ret == OK) { errx(0, "PASS"); + } else { errx(1, "FAIL"); } @@ -1226,10 +1263,13 @@ void reset() { int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); @@ -1286,6 +1326,7 @@ hmc5883_main(int argc, char *argv[]) if (!strcmp(argv[1], "calibrate")) { if (hmc5883::calibrate() == 0) { errx(0, "calibration successful"); + } else { errx(1, "calibration failed"); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index e9b60b01c..4915b81e3 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> @@ -317,6 +317,7 @@ L3GD20::init() _num_reports = 2; _oldest_report = _next_report = 0; _reports = new struct gyro_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -330,7 +331,7 @@ L3GD20::init() write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ @@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { @@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps) if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + } else { return -EINVAL; } @@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency <= 95) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; + } else if (frequency <= 190) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; + } else if (frequency <= 380) { _current_rate = 380; bits |= RATE_380HZ_LP_30HZ; + } else if (frequency <= 760) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; + } else { return -EINVAL; } @@ -746,17 +755,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -774,15 +787,17 @@ test() /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); + if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); @@ -793,7 +808,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); /* XXX add poll-rate tests here too */ @@ -808,10 +823,13 @@ void reset() { int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7b8a84f7e..90786886a 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> @@ -569,6 +569,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: @@ -592,12 +593,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -639,12 +640,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -976,17 +977,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -1006,21 +1011,24 @@ test() /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + ACCEL_DEVICE_PATH); /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -1033,10 +1041,11 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); + if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); @@ -1047,7 +1056,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1066,10 +1075,13 @@ void reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 4df9de94c..699cd36d2 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. */ @@ -299,6 +303,7 @@ MS5611::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -307,6 +312,7 @@ MS5611::init() /* get a publish handle on the baro topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + if (_baro_topic < 0) debug("failed to create sensor_baro object"); @@ -319,9 +325,10 @@ int MS5611::probe() { _retries = 10; - if((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + + if ((OK == probe_address(MS5611_ADDRESS_1)) || + (OK == probe_address(MS5611_ADDRESS_2))) { + _retries = 1; return OK; } @@ -480,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { @@ -514,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case BAROIOCSMSLPRESSURE: + /* range-check for sanity */ if ((arg < 80000) || (arg > 120000)) return -EINVAL; + _msl_pressure = arg; return OK; @@ -684,7 +694,7 @@ MS5611::collect() int64_t SENS2 = 5 * f >> 2; if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); + int64_t f2 = POW2(_TEMP + 1500); OFF2 += 7 * f2; SENS2 += 11 * f2 >> 1; } @@ -693,6 +703,7 @@ MS5611::collect() _OFF -= OFF2; _SENS -= SENS2; } + } else { /* pressure calculation, result in Pa */ @@ -810,8 +821,8 @@ MS5611::read_prom() uint16_t w; } cvt; - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are * called immediately after reset. */ usleep(3000); @@ -937,17 +948,22 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -964,11 +980,13 @@ test() int ret; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) err(1, "immediate read failed"); @@ -1021,10 +1039,13 @@ void reset() { int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); @@ -1057,6 +1078,7 @@ calibrate(unsigned altitude) float p1; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); @@ -1066,6 +1088,7 @@ calibrate(unsigned altitude) /* average a few measurements */ pressure = 0.0f; + for (unsigned i = 0; i < 20; i++) { struct pollfd fds; int ret; @@ -1087,6 +1110,7 @@ calibrate(unsigned altitude) pressure += report.pressure; } + pressure /= 20; /* average */ pressure /= 10; /* scale from millibar to kPa */ @@ -1104,8 +1128,10 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + exit(0); } @@ -1146,7 +1172,7 @@ ms5611_main(int argc, char *argv[]) errx(1, "missing altitude"); long altitude = strtol(argv[2], nullptr, 10); - + ms5611::calibrate(altitude); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 66db1c360..995c9393f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -110,7 +110,7 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? + // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO @@ -149,13 +149,13 @@ private: * group/index during mixing. */ static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); }; -namespace +namespace { PX4IO *g_dev; @@ -190,6 +190,7 @@ PX4IO::~PX4IO() /* spin waiting for the thread to stop */ unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -223,11 +224,13 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -235,6 +238,7 @@ PX4IO::init() /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -256,6 +260,7 @@ PX4IO::task_main() /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); + if (_serial_fd < 0) { debug("failed to open serial port for IO: %d", errno); _task = -1; @@ -343,6 +348,7 @@ PX4IO::task_main() _send_needed = true; } } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); @@ -364,9 +370,9 @@ PX4IO::task_main() int PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -458,13 +464,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + /* fake an update to the selected servo channel */ if ((arg >= 900) && (arg <= 2100)) { _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; _send_needed = true; + } else { ret = -EINVAL; } + break; case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): @@ -481,6 +490,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) delete _mixers; _mixers = nullptr; } + break; case MIXERIOCADDSIMPLE: { @@ -519,6 +529,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* allocate a new mixer group and load it from the file */ newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (newmixers->load_from_file(path) != 0) { delete newmixers; ret = -EINVAL; @@ -528,6 +539,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; } + _mixers = newmixers; } @@ -537,6 +549,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* not a recognised value */ ret = -ENOTTY; } + unlock(); return ret; @@ -576,6 +589,7 @@ px4io_main(int argc, char *argv[]) if (argc > 2) { fn[0] = argv[2]; fn[1] = nullptr; + } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; @@ -589,18 +603,24 @@ px4io_main(int argc, char *argv[]) switch (ret) { case OK: break; + case -ENOENT: errx(1, "PX4IO firmware file not found"); + case -EEXIST: case -EIO: errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + case -EINVAL: errx(1, "verify failed - retry the update"); + case -ETIMEDOUT: errx(1, "timed out waiting for bootloader - power-cycle and try again"); + default: errx(1, "unexpected error %d", ret); } + return ret; } diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 0fbbac839..5669aeb01 100644 --- a/apps/drivers/px4io/uploader.cpp +++ b/apps/drivers/px4io/uploader.cpp @@ -67,6 +67,7 @@ PX4IO_Uploader::upload(const char *filenames[]) int ret; _io_fd = open("/dev/ttyS2", O_RDWR); + if (_io_fd < 0) { log("could not open interface"); return -errno; @@ -74,6 +75,7 @@ PX4IO_Uploader::upload(const char *filenames[]) /* look for the bootloader */ ret = sync(); + if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); @@ -87,17 +89,20 @@ PX4IO_Uploader::upload(const char *filenames[]) log("failed to open %s", filenames[i]); continue; } + log("using firmware from %s", filenames[i]); break; } + if (_fw_fd == -1) return -ENOENT; /* do the usual program thing - allow for failure */ for (unsigned retries = 0; retries < 1; retries++) { if (retries > 0) { - log("retrying update..."); + log("retrying update..."); ret = sync(); + if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); @@ -106,25 +111,33 @@ PX4IO_Uploader::upload(const char *filenames[]) } ret = erase(); + if (ret != OK) { log("erase failed"); continue; } + ret = program(); + if (ret != OK) { log("program failed"); continue; } + ret = verify(); + if (ret != OK) { log("verify failed"); continue; } + ret = reboot(); + if (ret != OK) { log("reboot failed"); return ret; } + log("update complete"); ret = OK; @@ -145,6 +158,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) /* wait 100 ms for a character */ int ret = ::poll(&fds[0], 1, timeout); + if (ret < 1) { //log("poll timeout %d", ret); return -ETIMEDOUT; @@ -160,9 +174,11 @@ PX4IO_Uploader::recv(uint8_t *p, unsigned count) { while (count--) { int ret = recv(*p++); + if (ret != OK) return ret; } + return OK; } @@ -175,7 +191,7 @@ PX4IO_Uploader::drain() do { ret = recv(c, 10); //log("discard 0x%02x", c); - } while(ret == OK); + } while (ret == OK); } int @@ -184,6 +200,7 @@ PX4IO_Uploader::send(uint8_t c) //log("send 0x%02x", c); if (write(_io_fd, &c, 1) != 1) return -errno; + return OK; } @@ -192,9 +209,11 @@ PX4IO_Uploader::send(uint8_t *p, unsigned count) { while (count--) { int ret = send(*p++); + if (ret != OK) return ret; } + return OK; } @@ -205,15 +224,20 @@ PX4IO_Uploader::get_sync(unsigned timeout) int ret; ret = recv(c[0], timeout); + if (ret != OK) return ret; + ret = recv(c[1], timeout); + if (ret != OK) return ret; + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { log("bad sync 0x%02x,0x%02x", c[0], c[1]); return -EIO; } + return OK; } @@ -221,9 +245,11 @@ int PX4IO_Uploader::sync() { drain(); + /* complete any pending program operation */ for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) send(0); + send(PROTO_GET_SYNC); send(PROTO_EOC); return get_sync(); @@ -239,8 +265,10 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(PROTO_EOC); ret = recv((uint8_t *)&val, sizeof(val)); + if (ret != OK) return ret; + return get_sync(); } @@ -267,10 +295,13 @@ PX4IO_Uploader::program() /* get more bytes to program */ //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) return OK; + if (count < 0) return -errno; + ASSERT((count % 4) == 0); send(PROTO_PROG_MULTI); @@ -279,6 +310,7 @@ PX4IO_Uploader::program() send(PROTO_EOC); ret = get_sync(1000); + if (ret != OK) return ret; } @@ -297,6 +329,7 @@ PX4IO_Uploader::verify() send(PROTO_CHIP_VERIFY); send(PROTO_EOC); ret = get_sync(); + if (ret != OK) return ret; @@ -304,19 +337,24 @@ PX4IO_Uploader::verify() /* get more bytes to verify */ int base = (int)lseek(_fw_fd, 0, SEEK_CUR); count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) break; + if (count < 0) return -errno; + ASSERT((count % 4) == 0); send(PROTO_READ_MULTI); send(count); send(PROTO_EOC); + for (ssize_t i = 0; i < count; i++) { uint8_t c; ret = recv(c); + if (ret != OK) { log("%d: got %d waiting for bytes", base + i, ret); return ret; @@ -327,12 +365,15 @@ PX4IO_Uploader::verify() return -EINVAL; } } + ret = get_sync(); + if (ret != OK) { log("timeout waiting for post-verify sync"); return ret; } } + return OK; } @@ -358,6 +399,7 @@ PX4IO_Uploader::compare(bool &identical) send(PROTO_CHIP_VERIFY); send(PROTO_EOC); ret = get_sync(); + if (ret != OK) return ret; @@ -365,6 +407,7 @@ PX4IO_Uploader::compare(bool &identical) send(sizeof(fw_vectors)); send(PROTO_EOC); ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors)); + if (ret != OK) return ret; diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile new file mode 100644 index 000000000..4ad57f413 --- /dev/null +++ b/apps/drivers/stm32/Makefile @@ -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. +# +############################################################################ + +# +# STM32 driver support code +# +# Modules in this directory are compiled for all STM32 targets. +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c new file mode 100644 index 000000000..1ac90b16d --- /dev/null +++ b/apps/drivers/stm32/drv_hrt.c @@ -0,0 +1,858 @@ +/**************************************************************************** + * + * 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_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 <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + +#ifdef CONFIG_HRT_TIMER + +/* HRT configuration */ +#if HRT_TIMER == 1 +# define HRT_TIMER_BASE STM32_TIM1_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +# if CONFIG_STM32_TIM1 +# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1 +# endif +#elif HRT_TIMER == 2 +# define HRT_TIMER_BASE STM32_TIM2_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM2 +# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +# if CONFIG_STM32_TIM2 +# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2 +# endif +#elif HRT_TIMER == 3 +# define HRT_TIMER_BASE STM32_TIM3_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM3 +# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +# if CONFIG_STM32_TIM3 +# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3 +# endif +#elif HRT_TIMER == 4 +# define HRT_TIMER_BASE STM32_TIM4_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM4 +# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +# if CONFIG_STM32_TIM4 +# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4 +# endif +#elif HRT_TIMER == 5 +# define HRT_TIMER_BASE STM32_TIM5_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM5 +# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +# if CONFIG_STM32_TIM5 +# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5 +# endif +#elif HRT_TIMER == 8 +# define HRT_TIMER_BASE STM32_TIM8_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +# if CONFIG_STM32_TIM8 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 +# endif +#elif HRT_TIMER == 9 +# define HRT_TIMER_BASE STM32_TIM9_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# if CONFIG_STM32_TIM9 +# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 +# endif +#elif HRT_TIMER == 10 +# define HRT_TIMER_BASE STM32_TIM10_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP +# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# if CONFIG_STM32_TIM10 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 +# endif +#elif HRT_TIMER == 11 +# define HRT_TIMER_BASE STM32_TIM11_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# if CONFIG_STM32_TIM11 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 +# endif +#else +# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y +#endif + +/* + * HRT clock must be a multiple of 1MHz greater than 1MHz + */ +#if (HRT_TIMER_CLOCK % 1000000) != 0 +# error HRT_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if HRT_TIMER_CLOCK <= 1000000 +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * 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 + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if HRT_TIMER_CHANNEL == 1 +# define rCCR_HRT rCCR1 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 2 +# define rCCR_HRT rCCR2 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 3 +# define rCCR_HRT rCCR3 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 4 +# define rCCR_HRT rCCR4 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */ +#else +# error HRT_TIMER_CHANNEL must be a value between 1 and 4 +#endif + +/* + * 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 +static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context); +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 + */ +#ifdef CONFIG_HRT_PPM +# if HRT_PPM_CHANNEL == 1 +# define rCCR_PPM rCCR1 /* capture register for PPM */ +# define DIER_PPM GTIM_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 */ +# 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 */ +# 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 */ +# 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 */ +# else +# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set +# endif + +/* + * PPM decoder tuning parameters + */ +# 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 */ + +/* decoded PPM buffer */ +#define PPM_MAX_CHANNELS 12 +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT unsigned ppm_decoded_channels; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +/* 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; + +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 */ + unsigned next_channel; + 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 /* CONFIG_HRT_PPM */ + +/* + * 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 void +hrt_tim_init(void) +{ + /* clock/power on our timer */ + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + + /* run the full span of the counter */ + rARR = 0xffff; + + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +#ifdef CONFIG_HRT_PPM +/* + * 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? */ + width = count - ppm.last_edge; + ppm.last_edge = count; + + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) + ppm_edge_next = 0; + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* export the last set of samples if we got something sensible */ + if (ppm.next_channel > 4) { + for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_decoded_channels = 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; + return; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) + ppm_pulse_next = 0; + + /* 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; + +} +#endif /* CONFIG_HRT_PPM */ + +/* + * Handle the compare interupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context) +{ + uint32_t status; + + /* grab the timer for latency tracking purposes */ + latency_actual = rCNT; + + /* copy interrupt status */ + status = rSR; + + /* ack the interrupts we just read */ + rSR = ~status; + +#ifdef CONFIG_HRT_PPM + + /* was this a PPM edge? */ + if (status & (SR_INT_PPM | SR_OVF_PPM)) + hrt_ppm_decode(status); + +#endif + + /* was this a timer tick? */ + if (status & SR_INT_HRT) { + + /* 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(); + } + + return OK; +} + +/* + * 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; + uint32_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 = irqsave(); + + /* get the current counter value */ + count = rCNT; + + /* + * 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); + + irqrestore(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; +} + +/* + * Initalise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef CONFIG_HRT_PPM + /* 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 = irqsave(); + + /* if the entry is currently queued, remove it */ + 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); + + irqrestore(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 = irqsave(); + + 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; + + irqrestore(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) { + 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 */ + rCCR_HRT = latency_baseline = deadline & 0xffff; +} + +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]++; +} + + +#endif /* CONFIG_HRT_TIMER */ diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c new file mode 100644 index 000000000..e3801a417 --- /dev/null +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -0,0 +1,308 @@ +/**************************************************************************** + * + * 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.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include "drv_pwm_servo.h" + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + + +/* default rate (in Hz) of PWM updates */ +static uint32_t pwm_update_rate = 50; + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) + +static void +pwm_timer_init(unsigned timer) +{ + /* enable the timer clock before we try to talk to it */ + modifyreg32(pwm_timers[timer].clock_register, 0, pwm_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; + rCCER(timer) = 0; + rDCR(timer) = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; + + /* and update at the desired rate */ + rARR(timer) = (1000000 / pwm_update_rate) - 1; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + /* note that the timer is left disabled - arming is performed separately */ +} + +static void +pwm_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; +} + +static void +pwm_channel_init(unsigned channel) +{ + unsigned timer = pwm_channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(pwm_channels[channel].gpio); + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (6 << 4); + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 0); + break; + + case 2: + rCCMR1(timer) |= (6 << 12); + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 4); + break; + + case 3: + rCCMR2(timer) |= (6 << 4); + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 8); + break; + + case 4: + rCCMR2(timer) |= (6 << 12); + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 12); + break; + } +} + +int +up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_set: bogus channel %u\n", channel); + return -1; + } + + unsigned timer = pwm_channels[channel].timer_index; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].gpio == 0)) + return -1; + + /* configure the channel */ + if (value > 0) + value--; + + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} + +servo_position_t +up_pwm_servo_get(unsigned channel) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_get: bogus channel %u\n", channel); + return 0; + } + + unsigned timer = pwm_channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].gpio == 0)) + return 0; + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + return value; +} + +int +up_pwm_servo_init(uint32_t channel_mask) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + /* don't do init for disabled channels; this leaves the pin configs alone */ + if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0)) + pwm_channel_init(i); + } + + return OK; +} + +void +up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int +up_pwm_servo_set_rate(unsigned rate) +{ + if ((rate < 50) || (rate > 400)) + return -ERANGE; + + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + pwm_timer_set_rate(i, rate); + } + + return OK; +} + +void +up_pwm_servo_arm(bool armed) +{ + /* + * XXX this is inelgant and in particular will either jam outputs at whatever level + * they happen to be at at the time the timers stop or generate runts. + * The right thing is almost certainly to kill auto-reload on the timers so that + * they just stop at the end of their count for disable, and to reset/restart them + * for enable. + */ + + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + rCR1(i) = armed ? GTIM_CR1_CEN : 0; + } +} diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/apps/drivers/stm32/drv_pwm_servo.h new file mode 100644 index 000000000..667283424 --- /dev/null +++ b/apps/drivers/stm32/drv_pwm_servo.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 <drivers/drv_pwm_output.h> + +/* configuration limits */ +#define PWM_SERVO_MAX_TIMERS 2 +#define PWM_SERVO_MAX_CHANNELS 8 + +/* array of timers dedicated to PWM servo use */ +struct pwm_servo_timer { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; +}; + +/* supplied by board-specific code */ +__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; + +/* array of channels in logical order */ +struct pwm_servo_channel { + uint32_t gpio; + uint8_t timer_index; + uint8_t timer_channel; + servo_position_t default_value; +}; + +__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index bfde83cde..03cf3ee5d 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -34,8 +34,8 @@ /** * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * @@ -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> @@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = { {{TONE_NOTE_C7, 100}}, {{TONE_NOTE_D7, 100}}, {{TONE_NOTE_E7, 100}}, - { //This is tetris ;) + { + //This is tetris ;) {TONE_NOTE_C6, 40}, {TONE_NOTE_G5, 20}, {TONE_NOTE_G5S, 20}, @@ -361,6 +362,7 @@ ToneAlarm::init() int ret; ret = CDev::init(); + if (ret != OK) return ret; @@ -368,34 +370,34 @@ ToneAlarm::init() stm32_configgpio(GPIO_TONE_ALARM); /* clock/power on our timer */ - modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = 0; - rCCER &= TONE_CCER; /* unlock CCMR* registers */ - rCCMR1 = TONE_CCMR1; - rCCMR2 = TONE_CCMR2; - rCCER = TONE_CCER; - rDCR = 0; - - /* toggle the CC output each time the count passes 1 */ - TONE_rCCR = 1; - - /* - * Configure the timebase to free-run at half max frequency. - * XXX this should be more flexible in order to get a better - * frequency range, but for the F4 with the APB1 timers based - * at 42MHz, this gets us down to ~320Hz or so. - */ - rPSC = 1; - - /* make sure the timer is running */ - rCR1 = GTIM_CR1_CEN; - - debug("ready"); + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = 0; + rCCER &= TONE_CCER; /* unlock CCMR* registers */ + rCCMR1 = TONE_CCMR1; + rCCMR2 = TONE_CCMR2; + rCCER = TONE_CCER; + rDCR = 0; + + /* toggle the CC output each time the count passes 1 */ + TONE_rCCR = 1; + + /* + * Configure the timebase to free-run at half max frequency. + * XXX this should be more flexible in order to get a better + * frequency range, but for the F4 with the APB1 timers based + * at 42MHz, this gets us down to ~320Hz or so. + */ + rPSC = 1; + + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; + + debug("ready"); return OK; } @@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) switch (cmd) { case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); + if (new_pattern == 0) { /* cancel any current alarm */ _current_pattern = _pattern_none; @@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) /* and start playing it */ next(); + } else { /* current pattern is higher priority than the new pattern, ignore */ } + break; + default: result = -ENOTTY; break; @@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) /* sanity-check the size of the write */ if (len > (_max_pattern_len * sizeof(tone_note))) return -EFBIG; + if ((len % sizeof(tone_note)) || (len == 0)) return -EIO; + if (!check((tone_note *)buffer)) return -EIO; @@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) debug("starting user pattern"); next(); } + irqrestore(flags); return len; @@ -511,18 +520,22 @@ ToneAlarm::next(void) /* find the note to play */ if (_current_pattern == _pattern_user) { np = &_user_pattern[_next_note]; + } else { np = &_patterns[_current_pattern - 1][_next_note]; } /* work out which note is next */ _next_note++; + if (_next_note >= _note_max) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_END) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_REPEAT) { /* next note is a repeat, rewind in preparation */ _next_note = 0; @@ -534,11 +547,11 @@ ToneAlarm::next(void) /* set reload based on the pitch */ rARR = _notes[np->pitch]; - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + /* force an update, reloads the counter and all registers */ + rEGR = GTIM_EGR_UG; - /* enable the output */ - rCCER |= TONE_CCER; + /* enable the output */ + rCCER |= TONE_CCER; } /* arrange a callback when the note/rest is done */ @@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern) if ((i == 0) && ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT))) return false; + if (pattern[i].duration == DURATION_END) break; @@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern) if (pattern[i].pitch >= _note_max) return false; } + return true; } @@ -592,13 +607,16 @@ play_pattern(unsigned pattern) int fd, ret; fd = open("/dev/tone_alarm", 0); + if (fd < 0) err(1, "/dev/tone_alarm"); warnx("playing pattern %u", pattern); ret = ioctl(fd, TONE_SET_ALARM, pattern); + if (ret != 0) err(1, "TONE_SET_ALARM"); + exit(0); } @@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[]) 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"); @@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[]) if ((argc > 1) && !strcmp(argv[1], "start")) play_pattern(1); + if ((argc > 1) && !strcmp(argv[1], "stop")) play_pattern(0); + if ((pattern = strtol(argv[1], nullptr, 10)) != 0) play_pattern(pattern); 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 2632aebab..f029a9e37 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/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 6d990c46b..eccdeb78e 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -61,8 +61,6 @@ #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> -#include <arch/board/up_pwm_servo.h> - #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.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..54d2f6a0b 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -52,15 +52,13 @@ #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> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> -#include <arch/board/up_adc.h> - #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index 752c01986..745f76852 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -67,10 +67,12 @@ bl_update_main(int argc, char *argv[]) setopt(); int fd = open(argv[1], O_RDONLY); + if (fd < 0) err(1, "open %s", argv[1]); struct stat s; + if (stat(argv[1], &s) < 0) err(1, "stat %s", argv[1]); @@ -79,14 +81,17 @@ bl_update_main(int argc, char *argv[]) errx(1, "%s: file too large", argv[1]); uint8_t *buf = malloc(s.st_size); + if (buf == NULL) errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); if (read(fd, buf, s.st_size) != s.st_size) err(1, "firmware read error"); + close(fd); uint32_t *hdr = (uint32_t *)buf; + if ((hdr[0] < 0x20000000) || /* stack not below RAM */ (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ (hdr[1] < 0x08000000) || /* entrypoint not below flash */ @@ -123,6 +128,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: erase error 0x%02x", *sr); goto flash_end; @@ -148,6 +154,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: program error 0x%02x", *sr); goto flash_end; @@ -203,6 +210,7 @@ setopt(void) if ((*optcr & opt_mask) == opt_bits) errx(0, "option bits set"); + errx(1, "option bits setting failed; readback 0x%04x", *optcr); }
\ No newline at end of file diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 3cded52fa..79149caa0 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -135,20 +135,19 @@ * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. */ -struct at24c_dev_s -{ - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; }; /************************************************************************************ @@ -159,9 +158,9 @@ struct at24c_dev_s static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); + size_t nblocks, FAR uint8_t *buf); static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); + size_t nblocks, FAR const uint8_t *buf); static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); /************************************************************************************ @@ -180,35 +179,32 @@ static struct at24c_dev_s g_at24c; static int at24c_eraseall(FAR struct at24c_dev_s *priv) { - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2],0xff,priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) - { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) - { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; } /************************************************************************************ @@ -217,9 +213,9 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv) static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) { - /* EEprom need not erase */ + /* EEprom need not erase */ - return (int)nblocks; + return (int)nblocks; } /************************************************************************************ @@ -227,90 +223,86 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc ************************************************************************************/ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) + size_t nblocks, FAR uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) - { - return 0; - } - - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) - { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - if (--tries == 0) - { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -322,81 +314,75 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, ************************************************************************************/ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) + FAR const uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE+2]; - int ret; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; + blocksleft = nblocks; - if (startblock >= priv->npages) - { - return 0; - } + if (startblock >= priv->npages) { + return 0; + } - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); - for (;;) - { + for (;;) { - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); - if (ret >= 0) - break; + if (ret >= 0) + break; - fvdbg("write stall"); - usleep(1000); + fvdbg("write stall"); + usleep(1000); - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) - { - perf_count(priv->perf_write_errors); - return ERROR; - } - } + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } - startblock++; - buffer += priv->pagesize; - } + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -406,67 +392,65 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) - { - case MTDIOC_GEOMETRY: - { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - if (geo) - { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; #else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; #endif - ret = OK; + ret = OK; - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; - case MTDIOC_BULKERASE: - ret=at24c_eraseall(priv); - break; + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } - return ret; + return ret; } /************************************************************************************ @@ -483,46 +467,45 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * ************************************************************************************/ -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) -{ - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - if (priv) - { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; } /* @@ -530,5 +513,5 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) */ int at24c_nuke(void) { - return at24c_eraseall(&g_at24c); + return at24c_eraseall(&g_at24c); } diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index fa88fa09e..19a14aa02 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -143,13 +143,13 @@ eeprom_start(void) if (ret < 0) errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - + /* mount the EEPROM */ ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); if (ret < 0) errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - + started = true; warnx("mounted EEPROM at /eeprom"); exit(0); @@ -165,6 +165,7 @@ eeprom_erase(void) if (at24c_nuke()) errx(1, "erase failed"); + errx(0, "erase done, reboot now"); } @@ -190,7 +191,7 @@ eeprom_save(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); @@ -221,9 +222,9 @@ eeprom_load(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); - + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); int fd = open(name, O_RDONLY); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index 02c1bb133..15d448118 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> @@ -121,6 +121,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n"); exit(1); } @@ -129,7 +130,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -165,9 +166,11 @@ int led_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tled is running\n"); + } else { printf("\tled not started\n"); } + exit(0); } @@ -187,7 +190,7 @@ int led_thread_main(int argc, char *argv[]) while (!thread_should_exit) { /* swell blue led */ - + /* 200 Hz base loop */ usleep(1000000 / rate); diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 9d52557e7..3f52bdbf1 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -97,6 +97,7 @@ load(const char *devname, const char *fname) /* tell it to load the file */ ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + if (ret != 0) { fprintf(stderr, "failed loading %s\n", fname); } diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index d70d15da4..68dbd822e 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -70,10 +70,13 @@ param_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "save")) do_save(); + if (!strcmp(argv[1], "load")) do_load(); + if (!strcmp(argv[1], "import")) do_import(); + if (!strcmp(argv[1], "show")) do_show(); } @@ -154,8 +157,8 @@ do_show_print(void *arg, param_t param) float f; printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); /* * This case can be expanded to handle printing common structure types. @@ -167,19 +170,25 @@ do_show_print(void *arg, param_t param) printf("%d\n", i); return; } + break; + case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { printf("%4.4f\n", (double)f); return; } + break; + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param)); return; + default: printf("<unknown type %d>\n", 0 + param_type(param)); return; } + printf("<error fetching parameter %d>\n", param); } diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index f4e260211..59d2bc8f1 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -46,8 +46,8 @@ #include <string.h> #include <poll.h> -#include <arch/board/up_cpuload.h> -#include <arch/board/up_hrt.h> +#include <systemlib/cpuload.h> +#include <drivers/drv_hrt.h> /** * Start the top application. @@ -190,13 +190,15 @@ int top_main(int argc, char *argv[]) runtime_spaces = ""; } - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; unsigned stack_free = 0; uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + while (stack_free < stack_size) { if (*stack_sweeper++ != 0xff) break; + stack_free++; } 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/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 10b736fd6..75578d2ec 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -56,7 +56,7 @@ static int read_int8(bson_decoder_t decoder, int8_t *b) { - return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; + return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; } static int @@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder) while (decoder->pending > 0) { if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error discarding pending bytes"); + decoder->pending--; } /* get the type byte */ if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on type byte"); + decoder->node.type = tbyte; decoder->pending = 0; @@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder) /* get the node name */ nlen = 0; + for (;;) { if (nlen >= BSON_MAXNAME) CODER_KILL(decoder, "node name overflow"); + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) CODER_KILL(decoder, "read error on node name"); + if (decoder->node.name[nlen] == '\0') break; + nlen++; } @@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder) case BSON_INT: if (read_int32(decoder, &decoder->node.i)) CODER_KILL(decoder, "read error on BSON_INT"); + break; + case BSON_DOUBLE: if (read_double(decoder, &decoder->node.d)) CODER_KILL(decoder, "read error on BSON_DOUBLE"); + break; + case BSON_STRING: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + case BSON_BINDATA: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_BINDATA size"); + if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + decoder->node.subtype = tbyte; break; @@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) CODER_CHECK(decoder); /* if data already copied, return zero bytes */ - if (decoder->pending == 0) + if (decoder->pending == 0) return 0; /* copy bytes per the node size */ result = read(decoder->fd, buf, decoder->pending); + if (result != decoder->pending) CODER_KILL(decoder, "read error on copy_data"); @@ -209,7 +224,7 @@ static int write_int8(bson_encoder_t encoder, int8_t b) { debug("write_int8 %d", b); - return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; + return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; } static int @@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name) if (len > BSON_MAXNAME) return -1; + debug("write name '%s' len %d", name, len); return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1; } @@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char write_int32(encoder, len) || write(encoder->fd, name, len + 1) != (int)(len + 1)) CODER_KILL(encoder, "write error on BSON_STRING"); + return 0; } @@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_int8(encoder, subtype) || write(encoder->fd, data, size) != (int)(size)) CODER_KILL(encoder, "write error on BSON_BINDATA"); + return 0; } diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index 1b9de5cd3..b6229dc50 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -31,14 +31,14 @@ * ****************************************************************************/ - /** - * @file tinybson.h - * - * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org - * - * Some types and defines taken from the standalone BSON parser/generator - * in the Mongo C connector. - */ +/** +* @file tinybson.h +* +* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org +* +* Some types and defines taken from the standalone BSON parser/generator +* in the Mongo C connector. +*/ #ifndef _TINYBSON_H #define _TINYBSON_H @@ -77,8 +77,7 @@ typedef enum bson_binary_subtype { /** * Node structure passed to the callback. */ -typedef struct bson_node_s -{ +typedef struct bson_node_s { char name[BSON_MAXNAME]; bson_type_t type; bson_binary_subtype_t subtype; @@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t; */ typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); -struct bson_decoder_s -{ +struct bson_decoder_s { int fd; bson_decoder_callback callback; void *private; @@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); /** * Encoder state structure. */ -typedef struct bson_encoder_s -{ +typedef struct bson_encoder_s { int fd; } *bson_encoder_t; @@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i */ __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); -/** +/** * Append a string to the encoded stream. */ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 99ee41508..9105d83cb 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -45,13 +45,13 @@ int16_t int16_t_from_bytes(uint8_t bytes[]) { - union { - uint8_t b[2]; - int16_t w; - } u; + union { + uint8_t b[2]; + int16_t w; + } u; - u.b[1] = bytes[0]; - u.b[0] = bytes[1]; + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; - return u.w; + return u.w; } diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c new file mode 100644 index 000000000..20b711fa6 --- /dev/null +++ b/apps/systemlib/cpuload.c @@ -0,0 +1,176 @@ +/**************************************************************************** + * 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/err.c b/apps/systemlib/err.c index f22c5ed0d..3011743a1 100644 --- a/apps/systemlib/err.c +++ b/apps/systemlib/err.c @@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) fprintf(stderr, ": %s", strerror(errcode)); + fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lib_lowprintf("%s: ", getprogname()); @@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) lib_lowprintf(": %s", strerror(errcode)); + lib_lowprintf("\n"); #endif } @@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args) } void -warn(const char *fmt, ...) +warn(const char *fmt, ...) { va_list args; @@ -153,13 +157,13 @@ warn(const char *fmt, ...) } void -vwarn(const char *fmt, va_list args) +vwarn(const char *fmt, va_list args) { warnerr_core(errno, fmt, args); } void -warnc(int errcode, const char *fmt, ...) +warnc(int errcode, const char *fmt, ...) { va_list args; @@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...) } void -vwarnc(int errcode, const char *fmt, va_list args) +vwarnc(int errcode, const char *fmt, va_list args) { warnerr_core(errcode, fmt, args); } void -warnx(const char *fmt, ...) +warnx(const char *fmt, ...) { va_list args; @@ -183,7 +187,7 @@ warnx(const char *fmt, ...) } void -vwarnx(const char *fmt, va_list args) +vwarnx(const char *fmt, va_list args) { warnerr_core(NOCODE, fmt, args); } diff --git a/apps/systemlib/err.h b/apps/systemlib/err.h index f798b97e7..ca13d6265 100644 --- a/apps/systemlib/err.h +++ b/apps/systemlib/err.h @@ -36,30 +36,30 @@ * * Simple error/warning functions, heavily inspired by the BSD functions of * the same names. - * + * * The err() and warn() family of functions display a formatted error * message on the standard error output. In all cases, the last * component of the program name, a colon character, and a space are * output. If the fmt argument is not NULL, the printf(3)-like formatted * error message is output. The output is terminated by a newline * character. - * + * * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and * vwarnc() functions append an error message obtained from strerror(3) * based on a supplied error code value or the global variable errno, * preceded by another colon and space unless the fmt argument is NULL. - * + * * In the case of the errc(), verrc(), warnc(), and vwarnc() functions, * the code argument is used to look up the error message. - * + * * The err(), verr(), warn(), and vwarn() functions use the global * variable errno to look up the error message. - * + * * The errx() and warnx() functions do not append an error message. - * + * * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do * not return, but exit with the value of the argument eval. - * + * */ #ifndef _SYSTEMLIB_ERR_H @@ -71,18 +71,18 @@ __BEGIN_DECLS __EXPORT const char *getprogname(void); -__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn,format(printf,3, 4))); -__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0))); -__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf,1, 0))); -__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf,2, 3))); -__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf,2, 0))); -__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf,1, 0))); +__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); +__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0))); +__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3))); +__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); +__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); __END_DECLS diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index bc467bfa3..789368fbd 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current -// position is right of the track and negative if left of the track as seen from a point on the track line +// position is right of the track and negative if left of the track as seen from a point on the track line // headed towards the end point. crosstrack_error_s return_var; @@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, float bearing_end; float bearing_track; float bearing_diff; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; - + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; bearing_diff = _wrapPI(bearing_diff); - + // Return past_end = true if past end point of line - if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { return_var.past_end = true; return_var.error = false; return return_var; } - + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - return_var.distance = (dist_to_end)*sin(bearing_diff); - if(sin(bearing_diff) >=0 ) { + return_var.distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); } + return_var.error = false; - + return return_var; } -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, float bearing_sector_end; float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); bool in_sector; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; - - - if(arc_sweep >= 0) { + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; + + if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; } + in_sector = false; - + // Case where sector does not span zero - if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; - + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + // Case where sector does span zero - if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; - + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + // If in the sector then calculate distance and bearing to closest point - if(in_sector) { + if (in_sector) { return_var.past_end = false; float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - - if(dist_to_center <= radius) { + + if (dist_to_center <= radius) { return_var.distance = radius - dist_to_center; return_var.bearing = bearing_now + M_PI_F; + } else { return_var.distance = dist_to_center - radius; return_var.bearing = bearing_now; } - - // If out of the sector then calculate dist and bearing to start or end point + + // If out of the sector then calculate dist and bearing to start or end point + } else { - - // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) - // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to - // calculate the position of the start and end points. We should not be doing this often - // as this function generally will not be called repeatedly when we are out of the sector. - - // TO DO - this is messed up and won't compile + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile float start_disp_x = radius * sin(arc_start_bearing); float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); - float lon_start = lon_now + start_disp_x/111111.0d; - float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; - float lon_end = lon_now + end_disp_x/111111.0d; - float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - if(dist_to_start < dist_to_end) { + + if (dist_to_start < dist_to_end) { return_var.distance = dist_to_start; return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { return_var.past_end = true; return_var.distance = dist_to_end; @@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, } } - + return_var.bearing = _wrapPI(return_var.bearing); return_var.error = false; - return return_var; + return return_var; } float _wrapPI(float bearing) @@ -227,21 +238,25 @@ float _wrapPI(float bearing) while (bearing > M_PI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing <= -M_PI_F) { bearing = bearing + M_TWOPI_F; } + return bearing; } - + float _wrap2PI(float bearing) { while (bearing >= M_TWOPI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing < 0.0f) { bearing = bearing + M_TWOPI_F; } + return bearing; } @@ -251,23 +266,26 @@ float _wrap180(float bearing) while (bearing > 180.0f) { bearing = bearing - 360.0f; } + while (bearing <= -180.0f) { bearing = bearing + 360.0f; } + return bearing; } - + float _wrap360(float bearing) { while (bearing >= 360.0f) { bearing = bearing - 360.0f; } + while (bearing < 0.0f) { bearing = bearing + 360.0f; } + return bearing; } - -
\ No newline at end of file + diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 205afc79e..c98b4c85d 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -44,8 +44,8 @@ * @author Lorenz Meier <lm@inf.ethz.ch> * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ - - + + #include <stdbool.h> typedef struct { @@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// +// __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); - +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + float _wrap180(float bearing); -float _wrap360(float bearing); +float _wrap360(float bearing); float _wrapPI(float bearing); float _wrap2PI(float bearing);
\ No newline at end of file diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index b56226c03..6c1bbe469 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -63,7 +63,7 @@ float Mixer::get_control(uint8_t group, uint8_t index) { float value; - + _control_cb(_cb_handle, group, index, value); return value; diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 44a33bf8b..004151235 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; + } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; + } else if (!strcmp(geomname, "6x")) { geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "8+")) { geometry = MultirotorMixer::OCTA_PLUS; + } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + } else { debug("unrecognised geometry '%s'", geomname); return nullptr; } return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); } int diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index e2576a848..0e714ed50 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -57,7 +57,7 @@ #define CW (-1.0f) #define CCW (1.0f) -namespace +namespace { /* @@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space) pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust; + if (tmp > max) max = tmp; + outputs[i] = tmp; } /* scale values into the -1.0 - 1.0 range */ if (max > 1.0f) { fixup_scale = 2.0f / max; + } else { fixup_scale = 2.0f; } + for (unsigned i = 0; i < _rotor_count; i++) outputs[i] = -1.0f + (outputs[i] * fixup_scale); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 9e886ea65..eeb867f11 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" @@ -189,6 +189,7 @@ param_notify_changes(void) */ if (param_topic == -1) { param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } @@ -455,6 +456,7 @@ param_reset(param_t param) utarray_erase(param_values, pos, 1); } } + param_unlock(); if (s != NULL) @@ -560,8 +562,7 @@ out: return result; } -struct param_import_state -{ +struct param_import_state { bool mark_saved; }; @@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved) do { result = bson_decoder_next(&decoder); - } while(result > 0); + } while (result > 0); out: + if (result < 0) debug("BSON error decoding parameters"); 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/pid/pid.c b/apps/systemlib/pid/pid.c index f9b50d030..7e277cdc7 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float if (isfinite(kp)) { pid->kp = kp; + } else { ret = 1; } if (isfinite(ki)) { pid->ki = ki; + } else { ret = 1; } if (isfinite(kd)) { pid->kd = kd; + } else { ret = 1; } if (isfinite(intmax)) { pid->intmax = intmax; + } else { ret = 1; } - + if (isfinite(limit)) { pid->limit = limit; + } else { ret = 1; } @@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) - { + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; pid->sp = sp; - + // Calculated current error value float error = pid->sp - val; - + if (isfinite(error)) { // Why is this necessary? DEW pid->error_previous = error; } @@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / dt; + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; + } else { d = 0.0f; } // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabs(i) > pid->intmax ) - { + + if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabs(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; + } else { if (!isfinite(i)) { i = 0; } + pid->integral = i; pid->saturated = 0; } // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (output > pid->limit) output = pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c new file mode 100644 index 000000000..a5d2f738d --- /dev/null +++ b/apps/systemlib/ppm_decode.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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..c2b24321a --- /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/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 94a5283f0..50ac62464 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma pid = task_create(name, priority, stack_size, entry, argv); if (pid > 0) { - + /* configure the scheduler */ struct sched_param param; @@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma /* XXX do any other private task accounting here before the task starts */ } + sched_unlock(); return pid; @@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info) statres = stat("/dev/bma180", &sb); if (statres == OK) { - /* BMA180 indicates a v1.5-v1.6 board */ - strcpy(info->board_name, "FMU v1.6"); - info->board_version = 16; + /* BMA180 indicates a v1.5-v1.6 board */ + strcpy(info->board_name, "FMU v1.6"); + info->board_version = 16; + } else { statres = stat("/dev/accel", &sb); + if (statres == OK) { /* MPU-6000 indicates a v1.7+ board */ strcpy(info->board_name, "FMU v1.7"); info->board_version = 17; + } else { /* If no BMA and no MPU is present, it is a v1.3 board */ strcpy(info->board_name, "FMU v1.3"); 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> |