Browse Source

io_timer driver:Only support capture if DIRECT_INPUT_TIMER_CHANNELS > 0

This removes the unused input capture capabilities from the px4io
   and saves 128 bytes of ram.
sbg
David Sidrane 8 years ago committed by Lorenz Meier
parent
commit
f7835a8677
  1. 7
      src/drivers/boards/common/board_common.h
  2. 13
      src/drivers/px4fmu/fmu.cpp
  3. 5
      src/drivers/stm32/drv_input_capture.c
  4. 14
      src/drivers/stm32/drv_io_timer.c

7
src/drivers/boards/common/board_common.h

@ -185,6 +185,13 @@ @@ -185,6 +185,13 @@
# define BOARD_EEPROM_WP_CTRL(on_true)
#endif
/*
* Defined when a board has capture and uses channels.
*/
#if defined(DIRECT_INPUT_TIMER_CHANNELS) && DIRECT_INPUT_TIMER_CHANNELS > 0
#define BOARD_HAS_CAPTURE 1
#endif
/************************************************************************************
* Public Data

13
src/drivers/px4fmu/fmu.cpp

@ -579,12 +579,15 @@ PX4FMU::set_mode(Mode mode) @@ -579,12 +579,15 @@ PX4FMU::set_mode(Mode mode)
_num_outputs = 1;
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
up_input_capture_set(2, Rising, 0, NULL, NULL);
up_input_capture_set(3, Rising, 0, NULL, NULL);
DEVICE_DEBUG("MODE_2PWM2CAP");
// no break
#endif
// no break
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_2PWM");
@ -598,11 +601,14 @@ PX4FMU::set_mode(Mode mode) @@ -598,11 +601,14 @@ PX4FMU::set_mode(Mode mode)
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM1CAP");
up_input_capture_set(3, Rising, 0, NULL, NULL);
// no break
#endif
// no break
case MODE_3PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM");
@ -2701,6 +2707,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) @@ -2701,6 +2707,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
#if defined(BOARD_HAS_CAPTURE)
lock();
input_capture_config_t *pconfig = 0;
@ -2822,6 +2830,7 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) @@ -2822,6 +2830,7 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
}
unlock();
#endif
return ret;
}

5
src/drivers/stm32/drv_input_capture.c

@ -79,6 +79,10 @@ @@ -79,6 +79,10 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
#if defined(BOARD_HAS_CAPTURE)
/* Support Input capture */
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
@ -499,3 +503,4 @@ int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, b @@ -499,3 +503,4 @@ int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, b
return rv;
}
#endif // defined(BOARD_HAS_CAPTURE)

14
src/drivers/stm32/drv_io_timer.c

@ -132,6 +132,10 @@ typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ @@ -132,6 +132,10 @@ typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
static io_timer_allocation_t once = 0;
#if defined(BOARD_HAS_CAPTURE)
/* Stats and handlers are only useful for Capture */
typedef struct channel_stat_t {
uint32_t isr_cout;
uint32_t overflows;
@ -143,10 +147,12 @@ static struct channel_handler_entry { @@ -143,10 +147,12 @@ static struct channel_handler_entry {
channel_handler_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
#endif // defined(BOARD_HAS_CAPTURE)
static int io_timer_handler(uint16_t timer_index)
{
#if defined(BOARD_HAS_CAPTURE)
/* Read the count at the time of the interrupt */
uint16_t count = rCNT(timer_index);
@ -198,6 +204,7 @@ static int io_timer_handler(uint16_t timer_index) @@ -198,6 +204,7 @@ static int io_timer_handler(uint16_t timer_index)
/* Clear all the SR bits for interrupt enabled channels only */
rSR(timer_index) = ~(statusr & (enabled | enabled << 8));
#endif // defined(BOARD_HAS_CAPTURE)
return 0;
}
@ -651,10 +658,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, @@ -651,10 +658,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
gpio = timer_io_channels[channel].gpio_in;
break;
#if defined(BOARD_HAS_CAPTURE)
case IOTimerChanMode_Capture:
setbits = CCMR_C1_CAPTURE_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
#endif
case IOTimerChanMode_NotUsed:
setbits = 0;
@ -721,9 +731,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, @@ -721,9 +731,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
rvalue |= setbits;
rCCER(timer) = rvalue;
#if !defined(BOARD_HAS_CAPTURE)
UNUSED(dier_setbits);
#else
channel_handlers[channel].callback = channel_handler;
channel_handlers[channel].context = context;
rDIER(timer) |= dier_setbits << shifts;
#endif
px4_leave_critical_section(flags);
}

Loading…
Cancel
Save