From a4da48cb7883b37328db4b64550158e8cb146be3 Mon Sep 17 00:00:00 2001 From: "Andrew C. Smith" Date: Thu, 3 Jul 2014 19:53:25 -0400 Subject: [PATCH] Combined update, input_pwm updated, rcS and rc.sensors updated to include aerocore checks, IMU axis update. --- ROMFS/px4fmu_common/init.d/rc.sensors | 7 + ROMFS/px4fmu_common/init.d/rcS | 30 +- makefiles/config_aerocore_default.mk | 3 + src/drivers/boards/aerocore/aerocore_init.c | 15 + src/drivers/boards/aerocore/aerocore_input_pwm.c | 111 ++++ src/drivers/boards/aerocore/aerocore_pwm_servo.c | 3 +- src/drivers/boards/aerocore/board_config.h | 52 +- src/drivers/boards/aerocore/module.mk | 1 + src/drivers/drv_input_pwm.h | 77 +++ src/drivers/input_pwm/input_pwm.cpp | 706 ++++++++++++++++++++++ src/drivers/input_pwm/module.mk | 43 ++ src/drivers/l3gd20/l3gd20.cpp | 7 + src/drivers/lsm303d/lsm303d.cpp | 14 + src/drivers/px4fmu/fmu.cpp | 10 +- src/drivers/stm32/drv_input_pwm_channels.c | 306 ++++++++++ src/drivers/stm32/drv_input_pwm_channels.h | 69 +++ src/drivers/stm32/module.mk | 5 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 12 +- src/modules/dataman/dataman.c | 6 +- src/systemcmds/mtd/mtd.c | 6 +- 20 files changed, 1439 insertions(+), 44 deletions(-) create mode 100644 src/drivers/boards/aerocore/aerocore_input_pwm.c create mode 100644 src/drivers/drv_input_pwm.h create mode 100644 src/drivers/input_pwm/input_pwm.cpp create mode 100644 src/drivers/input_pwm/module.mk create mode 100644 src/drivers/stm32/drv_input_pwm_channels.c create mode 100644 src/drivers/stm32/drv_input_pwm_channels.h diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 1e14930..1ff951a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -29,6 +29,13 @@ then echo "[init] Using LSM303D" fi fi +if ver hwcmp AEROCORE +then + if lsm303d start + then + echo "[init] Using LSM303D" + fi +fi # Start airspeed sensors if meas_airspeed start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d06f89..49eccd7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -64,7 +64,12 @@ then # # Start CDC/ACM serial driver # - sercon + if ver hwcmp AEROCORE + then + echo "[init] CDC/ACM serial driver not loaded" + else + sercon + fi # # Start the ORB (first app to start) @@ -272,8 +277,13 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & + if ver hwcmp AEROCORE + then + # do nothing, console on UART8 + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi # # Start the Commander (needs to be this early for in-air-restarts) @@ -312,6 +322,20 @@ then tone_alarm $TUNE_OUT_ERROR fi + if ver hwcmp AEROCORE + then + if param compare AEROCORE_RC 1 + then + echo "[init] Starting PWM RC input" + input_pwm start + fi + if param compare AEROCORE_RC 2 + then + echo "[init] Starting Spektrum RC input" + # not implemented yet + fi + fi + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 53a2ad1..8a7d68d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -23,6 +23,8 @@ MODULES += drivers/ms5611 MODULES += drivers/gps MODULES += drivers/hil MODULES += modules/sensors +MODULES += drivers/mkblctrl +MODULES += drivers/input_pwm # # System commands @@ -40,6 +42,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver # # General system control diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 4e3ba2d..2635c4b 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -70,6 +70,20 @@ #include #include +#include + +/** +* Configure the RC input for the AeroCore +* +* 1 = pwm (starts input_pwm app) +* 2 = spektrum (starts input_spektrum app) +* 0 = no app started (no RC input or PPM use through fmu app) +* +* @min = 0 +* @max = 1 +* @group = AeroCore +*/ +PARAM_DEFINE_INT32(AEROCORE_RC, 0); /**************************************************************************** * Pre-Processor Definitions @@ -280,3 +294,4 @@ __EXPORT int nsh_archinitialize(void) return OK; } + diff --git a/src/drivers/boards/aerocore/aerocore_input_pwm.c b/src/drivers/boards/aerocore/aerocore_input_pwm.c new file mode 100644 index 0000000..504e4c1 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_input_pwm.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 aerocore_input_pwm.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct input_pwm_timer input_pwm_timers[INPUT_PWM_MAX_TIMERS] = { + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .irq_vector = STM32_IRQ_TIM4 + }, + { + .base = STM32_TIM5_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM5EN, + .clock_freq = STM32_APB1_TIM5_CLKIN, + .irq_vector = STM32_IRQ_TIM5 + }, +}; + +__EXPORT const struct input_pwm_channel input_pwm_channels[INPUT_PWM_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM4_CH4IN, + .timer_index = 0, + .timer_channel = 4, + }, + { + .gpio = GPIO_TIM4_CH3IN, + .timer_index = 0, + .timer_channel = 3, + }, + { + .gpio = GPIO_TIM4_CH2IN, + .timer_index = 0, + .timer_channel = 2, + }, + { + .gpio = GPIO_TIM4_CH1IN, + .timer_index = 0, + .timer_channel = 1, + }, + { + .gpio = GPIO_TIM5_CH4IN, + .timer_index = 1, + .timer_channel = 4, + }, + { + .gpio = GPIO_TIM5_CH3IN, + .timer_index = 1, + .timer_channel = 3, + }, + { + .gpio = GPIO_TIM5_CH2IN, + .timer_index = 1, + .timer_channel = 2, + }, + { + .gpio = GPIO_TIM5_CH1IN, + .timer_index = 1, + .timer_channel = 1, + } +}; diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c index 251eaff..17db31a 100644 --- a/src/drivers/boards/aerocore/aerocore_pwm_servo.c +++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c @@ -84,12 +84,13 @@ __EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { .timer_channel = 3, .default_value = 1500, }, +/* NOT FUNCTIONAL { .gpio = GPIO_TIM1_CH4OUT, .timer_index = 0, .timer_channel = 4, .default_value = 1500, - }, + },*/ { .gpio = GPIO_TIM3_CH1OUT, .timer_index = 1, diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 70142a3..1facb11 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -90,27 +90,11 @@ __BEGIN_DECLS /* User GPIOs broken out on J11 */ #define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) #define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) -#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_PORTD|GPIO_PIN12) -#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) -#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) -#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) #define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) -#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_PORTD|GPIO_PIN12) -#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) -#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) -#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) /* PWM * @@ -136,15 +120,39 @@ __BEGIN_DECLS #define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 #define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 +/* PWM + * + * Eight PWM inputs are configured. + * + * Pins: + * + * CH8 : PA0 : TIM5_CH1 + * CH7 : PA1 : TIM5_CH2 + * CH6 : PA2 : TIM5_CH3 + * CH5 : PA3 : TIM5_CH4 + * CH4 : PD12 : TIM4_CH1 + * CH3 : PD13 : TIM4_CH2 + * CH2 : PD14 : TIM4_CH3 + * CH1 : PD15 : TIM4_CH4 + */ +#define GPIO_TIM5_CH1IN GPIO_TIM5_CH1IN_1 +#define GPIO_TIM5_CH2IN GPIO_TIM5_CH2IN_1 +#define GPIO_TIM5_CH3IN GPIO_TIM5_CH3IN_1 +#define GPIO_TIM5_CH4IN GPIO_TIM5_CH4IN_1 +#define GPIO_TIM4_CH1IN GPIO_TIM4_CH1IN_2 +#define GPIO_TIM4_CH2IN GPIO_TIM4_CH2IN_2 +#define GPIO_TIM4_CH3IN GPIO_TIM4_CH3IN_2 +#define GPIO_TIM4_CH4IN GPIO_TIM4_CH4IN_2 + /* High-resolution timer */ #define HRT_TIMER 8 /* use timer 8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ -/* Tone Alarm (no onboard speaker )*/ -#define TONE_ALARM_TIMER 2 /* timer 2 */ +/* Tone Alarm (no onboard speaker)*/ +#define TONE_ALARM_TIMER 10 /* timer 10 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF3|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTB|GPIO_PIN8) /**************************************************************************************************** diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index b53fe0a..6439f83 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -4,5 +4,6 @@ SRCS = aerocore_init.c \ aerocore_pwm_servo.c \ + aerocore_input_pwm.c \ aerocore_spi.c \ aerocore_led.c diff --git a/src/drivers/drv_input_pwm.h b/src/drivers/drv_input_pwm.h new file mode 100644 index 0000000..46d3d72 --- /dev/null +++ b/src/drivers/drv_input_pwm.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 input PWM interface. + * + */ + +#pragma once + +#include +#include + +#include "drv_orb_dev.h" +// use existing rc input driver info +#include "drv_rc_input.h" + +__BEGIN_DECLS + +/** + * Default minimum PWM in us + */ +#define INPUT_PWM_MIN 800 + +/** + * Default maximum PWM in us + */ +#define INPUT_PWM_MAX 2200 + +/* + * Low-level PWM input interface. + * + * This is the low-level API to the platform-specific PWM driver. + */ + +/** + * Intialise the PWM servo inputs 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_input_pwm_timer_init(uint8_t timer_index); +__EXPORT extern int up_input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t *value); + +__END_DECLS diff --git a/src/drivers/input_pwm/input_pwm.cpp b/src/drivers/input_pwm/input_pwm.cpp new file mode 100644 index 0000000..ccf4e83 --- /dev/null +++ b/src/drivers/input_pwm/input_pwm.cpp @@ -0,0 +1,706 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file input_pwm.cpp + * + * Driver for PWM RC inputs. Creates a device for each timer defined in board specific file. + * Channel number is defined by the order of the channels listed in board specific file (index). + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/* PWM signals are 20ms long, so max rate should be 50Hz */ +#define INPUT_PWM_INTERVAL 20000 /* microseconds */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/** PWM decoder state machine */ +struct pwmState { + uint16_t last_edge; /**< last capture time */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +}; + + +class InputPWM : public device::CDev +{ +public: + InputPWM(uint8_t timer_index, const char *devName); + virtual ~InputPWM(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + /* static member to hold pwm values for channels from all timers */ + /* written by everyone but read/reported by instance 0 */ + static volatile uint16_t input_pwm_channel_values[INPUT_PWM_MAX_CHANNELS]; + /* bool flags to indicate if the channel is active (decoded) */ + static volatile bool input_pwm_channel_decoded[INPUT_PWM_MAX_CHANNELS]; + +private: + work_s _work; + unsigned _measure_ticks; + + RingBuffer *_reports; + + int _temp_variable; + + orb_advert_t _input_rc_topic; + + perf_counter_t _sample_perf; + perf_counter_t _buffer_overflows; + + uint8_t _timer_index; /* index of timer in board config file */ + uint8_t _num_channels; /* number of channels tied to this timer */ + uint8_t _global_channel_index[INPUT_PWM_MAX_CHANNELS_PER_TIMER]; /* index of where this timer's channels lie globally */ + + volatile uint16_t _temp_rc_buffer[INPUT_PWM_MAX_CHANNELS_PER_TIMER]; /* stores PWM values as they're read in */ +// volatile uint8_t _decoded_channels; /* actual number of input channels seen */ + + pwmState _pwmChannels[INPUT_PWM_MAX_CHANNELS_PER_TIMER]; /* holds pwm decoder state */ + /* since each instance connects to a timer */ + /* max channels is 4 */ + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + void interrupt(void *ctx); + + void update_pwm_state(uint8_t timer_channel, uint16_t count); + +}; + +/* need to define our static pointer */ +volatile uint16_t InputPWM::input_pwm_channel_values[INPUT_PWM_MAX_CHANNELS]; +/* bool flags to indicate if the channel is active (decoded) */ +volatile bool InputPWM::input_pwm_channel_decoded[INPUT_PWM_MAX_CHANNELS]; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int input_pwm_main(int argc, char *argv[]); + +InputPWM::InputPWM(uint8_t timer_index, const char* devName) : + CDev("InputPWM", devName, input_pwm_timers[timer_index].irq_vector), + _measure_ticks(0), + _reports(nullptr), + _input_rc_topic(-1), + _temp_variable(2345), + _sample_perf(perf_alloc(PC_ELAPSED, "input_pwm_read")), + _buffer_overflows(perf_alloc(PC_COUNT, "input_pwm_buffer_overflows")), + _timer_index(timer_index) +{ + _debug_enabled = false; + + _pwmChannels[0].phase = pwmState::ARM; + _pwmChannels[1].phase = pwmState::ARM; + _pwmChannels[2].phase = pwmState::ARM; + _pwmChannels[3].phase = pwmState::ARM; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +InputPWM::~InputPWM() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_buffer_overflows); +} + +void +InputPWM::update_pwm_state(uint8_t timer_channel, uint16_t count) +{ + uint16_t width; + + /* how long since the last edge? - this handles counter wrapping implicitely. */ + width = count - _pwmChannels[timer_channel-1].last_edge; + + switch (_pwmChannels[timer_channel-1].phase) { + + case pwmState::ARM: + + /* frame length is everything including the start gap */ + _pwmChannels[timer_channel-1].phase = pwmState::ACTIVE; + break; + + case pwmState::ACTIVE: + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((width < INPUT_PWM_MIN) || (width > INPUT_PWM_MAX)) + { + /* missed an edge, stay ACTIVE and we'll catch it on the next go */ + } + else + { + /* if we have room to store the value, do so */ + /* store in local buffer -- used in read command */ + _temp_rc_buffer[timer_channel-1] = width; + /* store in global buffer */ + input_pwm_channel_values[_global_channel_index[timer_channel-1]] = width; + /* set decoded flag */ + input_pwm_channel_decoded[_global_channel_index[timer_channel-1]] = true; + _pwmChannels[timer_channel-1].phase = pwmState::ARM; + + } + break; + + } + + _pwmChannels[timer_channel-1].last_edge = count; + + return; +} + +int +InputPWM::init() +{ + int ret = ERROR; + + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); + return ERROR; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(rc_input_values)); + if (_reports == nullptr) + return ERROR; + + struct rc_input_values zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _input_rc_topic = orb_advertise(ORB_ID(input_rc), &zero_report); + + if (_input_rc_topic < 0) + debug("failed to create input_rc object"); + + int channel_count = 0; + /* loop through list of all channels and get this timer's channels */ + for (int i = 0;iinterrupt_enable(); + + ret = OK; +} + +ssize_t +InputPWM::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct rc_input_values); + struct rc_input_values *buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* time stamp it */ + buf->timestamp_publication = hrt_absolute_time(); + + /* record the actual number of received channels */ + buf->channel_count = _num_channels; + + /* no way to receive signal strength, set to 100% = 255 */ + buf->rssi = 255; + + /* set the input source to unknown */ + buf->input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* loop through the temp buffer and fill the report values */ + for (int i=0;i<_num_channels;i++) + buf->values[i] = _temp_rc_buffer[i]; + + return sizeof(struct rc_input_values); +} + +int +InputPWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(INPUT_PWM_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(INPUT_PWM_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000/TICK2USEC(_measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + return OK; + + case RC_INPUT_GET: { + /* fetch R/C input values into (rc_input_values *)arg */ + struct rc_input_values *report = (rc_input_values *)arg; + int ret; + ret = read(0, (char *)report, sizeof(*report)); + if (ret > 0) + return OK; + else + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +void +InputPWM::interrupt(void *ctx) +{ + uint8_t timer_channel; + uint16_t value[_num_channels]; + up_input_pwm_timer_isr(_timer_index, &timer_channel, value); + + /* parse the channel flags */ + for (int i=0;i<_num_channels;i++) + if (timer_channel & (1 << i)) + update_pwm_state(i+1, value[i]); +} + +void +InputPWM::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&InputPWM::cycle_trampoline, this, 1); +} + +void +InputPWM::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +InputPWM::cycle_trampoline(void *arg) +{ + InputPWM *dev = (InputPWM *)arg; + dev->cycle(); +} + +void +InputPWM::cycle() +{ + + int ret; + + perf_begin(_sample_perf); + + struct rc_input_values report; + + /* copy the current pwm values by reading them in */ + ret = read(0, (char *)&report, sizeof(report)); + if (ret < 0) { + /* couldn't read data, make empty report */ + report.timestamp_publication = hrt_absolute_time(); + report.channel_count = 0; + report.rssi = 0; + report.input_source = RC_INPUT_SOURCE_UNKNOWN; + if (_reports->force(&report)){ + perf_count(_buffer_overflows); + } + perf_end(_sample_perf); + return; + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + perf_end(_sample_perf); + + // only _timer_index = 0 should publish a report of all rc inputs + if (_timer_index == 0) { + int decoded_channels = 0; + report.timestamp_publication = hrt_absolute_time(); + report.timestamp_last_signal = report.timestamp_publication; + report.rssi = 255; + report.rc_failsafe = false; + report.rc_lost = false; + report.rc_lost_frame_count = 0; + report.rc_total_frame_count = 0; + report.rc_ppm_frame_length = 0; + report.input_source = RC_INPUT_SOURCE_UNKNOWN; + /* find out how many are actually active */ + for (int i=0;i < INPUT_PWM_MAX_CHANNELS; i++) { + if (input_pwm_channel_decoded[i]) { + report.values[i] = input_pwm_channel_values[i]; + decoded_channels++; + } + else + report.values[i] = 0; + } + report.channel_count = decoded_channels; + + /* publish the data */ + orb_publish(ORB_ID(input_rc), _input_rc_topic, &report); + /* notify anyone waiting for the data */ + poll_notify(POLLIN); + } + + /* schedule a fresh cycle call */ + work_queue(HPWORK, + &_work, + (worker_t)&InputPWM::cycle_trampoline, + this, + _measure_ticks); + +} + +void +InputPWM::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + + /* print out the rc channels */ + int channel_count = 0; + for (int i=0;iinit()) { + delete g_dev[i]; + warnx("Could not start rc_input device [%d]", i); + } + + + /* set the poll rate to default, starts automatic data collection */ + fd = open(devName, O_RDONLY); + + if (fd < 0 ) { + delete g_dev[i]; + warnx("Could not open rc_input device [%d]", i); + } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + delete g_dev[i]; + warnx("Could not set poll rate for rc_input device [%d]", i); + } + + num_timers++; + } + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + // just print out index 0 timer + if (g_dev[0] == nullptr) + errx(1, "driver not running"); + + for (int i=0;iprint_info(); + } + + exit(0); +} + +} // namespace + +int +input_pwm_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + input_pwm::start(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + input_pwm::info(); + + errx(1, "unrecognized command, try 'start' or 'info'"); +} diff --git a/src/drivers/input_pwm/module.mk b/src/drivers/input_pwm/module.mk new file mode 100644 index 0000000..481956a --- /dev/null +++ b/src/drivers/input_pwm/module.mk @@ -0,0 +1,43 @@ +#**************************************************************************** +# +# Copyright (C) 2014 Andrew C. Smith. All rights reserved. +# Author: Andrew C. Smith +# +# 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 AeroDroid 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. +# +# ****************************************************************************/ + +# +# Input_PWM driver +# + +MODULE_COMMAND = input_pwm + +MODULE_STACKSIZE = 2048 + +SRCS = input_pwm.cpp diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e7238..09da554 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -906,6 +906,13 @@ L3GD20::measure() report.z_raw = raw_report.z; +// the aerocore has the gyro mounted on the top, rotate 180 about x axis +#ifdef CONFIG_ARCH_BOARD_AEROCORE + report.x_raw = raw_report.x; + report.y_raw = ((raw_report.y==-32768) ? 32767 : -raw_report.y); + report.z_raw = ((raw_report.z==-32768) ? 32767 : -raw_report.z); +#endif + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dc..4d4feb9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1525,6 +1525,13 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; +// the aerocore has the accel mounted on the top, rotate 180 about x axis +#ifdef CONFIG_ARCH_BOARD_AEROCORE + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = ((raw_accel_report.y==-32768) ? 32767 : -raw_accel_report.y); + accel_report.z_raw = ((raw_accel_report.z==-32768) ? 32767 : -raw_accel_report.z); +#endif + float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; @@ -1603,6 +1610,13 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; + +// the aerocore has the mag mounted on the top, rotate 180 about x axis +#ifdef CONFIG_ARCH_BOARD_AEROCORE + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = ((raw_mag_report.y==-32768) ? 32767 : -raw_mag_report.y); + mag_report.z_raw = ((raw_mag_report.z==-32768) ? 32767 : -raw_mag_report.z); +#endif mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa1..113141a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -211,15 +211,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, - {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, - {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, - {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, - {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, #endif }; diff --git a/src/drivers/stm32/drv_input_pwm_channels.c b/src/drivers/stm32/drv_input_pwm_channels.c new file mode 100644 index 0000000..335ad84 --- /dev/null +++ b/src/drivers/stm32/drv_input_pwm_channels.c @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * 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_input_pwm.c + * + * PWM Input driver supporting PWM RC inputs connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have input pins, works with interrupts. + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drv_input_pwm_channels.h" + +#include +#include +#include + +#include +#include +#include + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(input_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) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) + +/** PWM decoder state machine */ +struct { + uint16_t last_edge; /**< last capture time */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} pwm; + +//static int input_pwm_timer_isr(int irq, void *context); +static int input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t *value); +static uint16_t input_pwm_decode(uint32_t status, uint8_t timer, uint8_t timer_channel); +static void input_pwm_timer_init(unsigned timer); +static void input_pwm_channel_init(unsigned channel); + +/** + * Handle the timer interrupt by reading and parsing the status register + * for the given timer. + * + * Return the channel number(s) in binary flags (0+2+3=>channel=12) + * and a pointer to an array 4 long uint16_t value[4] + */ +int +up_input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t value[4]) +{ + uint32_t status; + /* copy interrupt status */ + status = rSR(timer); + + /* ack the interrupts we just read */ + rSR(timer) = ~status; + + *channel = 0; + /* which channel was this */ + /* channel 1 */ + if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { + value[0] = input_pwm_decode(status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF), timer, 1); + *channel |= (1 << 0); + } + /* channel 2 */ + if (status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF)){ + value[1] = input_pwm_decode(status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF), timer, 2); + *channel |= (1 << 1); + } + /* channel 3 */ + if (status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF)){ + value[2] = input_pwm_decode(status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF), timer, 3); + *channel |= (1 << 2); + } + /* channel 4 */ + if (status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF)){ + value[3] = input_pwm_decode(status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF), timer, 4); + *channel |= (1 << 3); + } + return OK; +} + +uint16_t +input_pwm_decode(uint32_t status, uint8_t timer, uint8_t timer_channel) +{ + uint16_t count; + + // for now we don't care about CCxOF, state machine will take care of it + switch (timer_channel) { + + case 1: + count = rCCR1(timer); + break; + /* did we miss an edge? */ + if (status & GTIM_SR_CC1OF) + goto error; + + case 2: + count = rCCR2(timer); + break; + /* did we miss an edge? */ + if (status & GTIM_SR_CC2OF) + goto error; + + case 3: + count = rCCR3(timer); + break; + /* did we miss an edge? */ + if (status & GTIM_SR_CC3OF) + goto error; + + case 4: + count = rCCR4(timer); + break; + /* did we miss an edge? */ + if (status & GTIM_SR_CC4OF) + goto error; + + } + return count; + /* the state machine is corrupted; reset it */ +error: + /* we don't like the state of the decoder, reset it and try again */ + printf("overwrite error\n"); + return 0; +} + +static void +input_pwm_timer_init(unsigned timer) +{ + /* enable the timer clock before we try to talk to it */ + modifyreg32(input_pwm_timers[timer].clock_register, 0, input_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) = (input_pwm_timers[timer].clock_freq / 1000000) - 1; + + /* run the full span of the counter */ + rARR(timer) = 0xffff; + + /* generate an update event; reloads the counter, all registers */ + rEGR(timer) = GTIM_EGR_UG; + /* enable the timer and disable the update flag event */ + rCR1(timer) = GTIM_CR1_UDIS | GTIM_CR1_CEN; +} + +static void +input_pwm_channel_init(unsigned channel) +{ + unsigned timer = input_pwm_channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(input_pwm_channels[channel].gpio); + + /* configure the channel */ + switch (input_pwm_channels[channel].timer_channel) { + case 1: + rDIER(timer) |= GTIM_DIER_CC1IE; + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCCR1(timer) = 1000; + rCCER(timer) |= GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP; + break; + + case 2: + rDIER(timer) |= GTIM_DIER_CC2IE; + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCCR2(timer) = 1000; + rCCER(timer) |= GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP; + break; + + case 3: + rDIER(timer) |= GTIM_DIER_CC3IE; + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCCR3(timer) = 1000; + rCCER(timer) |= GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP; + break; + + case 4: + rDIER(timer) |= GTIM_DIER_CC4IE; + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + rCCR4(timer) = 1000; + rCCER(timer) |= GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP; + break; + } +} + +int +up_input_pwm_init() +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < INPUT_PWM_MAX_TIMERS; i++) { + if (input_pwm_timers[i].base != 0) + input_pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < INPUT_PWM_MAX_CHANNELS; i++) { + /* don't do init for disabled channels; this leaves the pin configs alone */ + if (input_pwm_channels[i].timer_channel != 0) + input_pwm_channel_init(i); + } + + return OK; +} + +/* + Initialize a timer referenced by timer_index, initialize its channels and return (ref) its irq vector +*/ + +int +up_input_pwm_timer_init(uint8_t timer_index) +{ + /* make sure timer_index is in bounds */ + if (timer_index < 0 || timer_index > INPUT_PWM_MAX_TIMERS) + return -1; + + /* do basic timer initialization first */ + if (input_pwm_timers[timer_index].base != 0) + input_pwm_timer_init(timer_index); + + /* now init channels associated with this timer */ + for (unsigned i = 0; i < INPUT_PWM_MAX_CHANNELS; i++) { + /* don't do init for disabled channels; this leaves the pin configs alone */ + if (input_pwm_channels[i].timer_index == timer_index) + input_pwm_channel_init(i); + } + + return OK; +} diff --git a/src/drivers/stm32/drv_input_pwm_channels.h b/src/drivers/stm32/drv_input_pwm_channels.h new file mode 100644 index 0000000..f423bfe --- /dev/null +++ b/src/drivers/stm32/drv_input_pwm_channels.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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_input_pwm.h + * + * stm32-specific input pwm capture. + */ + +#pragma once + +#include + +/* configuration limits */ +#define INPUT_PWM_MAX_TIMERS 3 +#define INPUT_PWM_MAX_CHANNELS_PER_TIMER 4 +#define INPUT_PWM_MAX_CHANNELS (INPUT_PWM_MAX_TIMERS * INPUT_PWM_MAX_CHANNELS_PER_TIMER) + +/* array of timers dedicated to PWM input use */ +struct input_pwm_timer { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; + int irq_vector; +}; + +/* array of channels in logical order */ +struct input_pwm_channel { + uint32_t gpio; + uint8_t timer_index; + uint8_t timer_channel; +}; + +/* supplied by board-specific code */ +__EXPORT extern const struct input_pwm_timer input_pwm_timers[INPUT_PWM_MAX_TIMERS]; +__EXPORT extern const struct input_pwm_channel input_pwm_channels[INPUT_PWM_MAX_CHANNELS]; + +int up_input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t *value); diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index bb751c7..518e147 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -38,6 +38,9 @@ # SRCS = drv_hrt.c \ - drv_pwm_servo.c + drv_pwm_servo.c +ifeq ($(CONFIG), aerocore_default) + SRCS += drv_input_pwm_channels.c +endif INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aa..486d26b 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -148,22 +148,22 @@ # endif #elif TONE_ALARM_TIMER == 9 # define TONE_ALARM_BASE STM32_TIM9_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM9_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM9EN # ifdef CONFIG_STM32_TIM9 # error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9 # endif #elif TONE_ALARM_TIMER == 10 # define TONE_ALARM_BASE STM32_TIM10_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM10_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM10EN # ifdef CONFIG_STM32_TIM10 # error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10 # endif #elif TONE_ALARM_TIMER == 11 # define TONE_ALARM_BASE STM32_TIM11_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM11_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN # ifdef CONFIG_STM32_TIM11 # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313..2fa4097 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -121,7 +121,11 @@ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; -static const char *k_data_manager_device_path = "/fs/microsd/dataman"; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + static const char *k_data_manager_device_path = "/fs/mtd_dataman"; +#else + static const char *k_data_manager_device_path = "/fs/microsd/dataman"; +#endif /* The data manager work queues */ diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaaf..29dac78 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -101,7 +101,11 @@ static struct mtd_dev_s *mtd_dev; static unsigned n_partitions_current = 0; /* note, these will be equally sized */ -static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints", "/fs/mtd_dataman"}; +#else + static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +#endif static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); int mtd_main(int argc, char *argv[]) -- 1.7.9.5