fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 16:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

src/drivers/{kinetis,stm32}: move to arch-specific directories

Browse Source
This commit is contained in:
Beat Küng
2019-08-30 07:59:44 +02:00
parent 3003e7d0fc
commit f3fccf53f6
129 changed files with 351 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/adc/adc.cpp
+1
View File
@@ -42,6 +42,7 @@
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_arch/adc.h>
#include <px4_config.h>
#include <px4_log.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
src/drivers/boards/common/stm32/CMakeLists.txt
-2
View File
@@ -39,7 +39,5 @@ add_library(drivers_boards_common_arch
board_critmon.c
)
add_dependencies(drivers_boards_common_arch prebuild_targets)
add_dependencies(drivers_boards_common_arch arch_adc)
target_compile_options(drivers_boards_common_arch PRIVATE -Wno-cast-align) # TODO: fix and enable
target_link_libraries(drivers_boards_common_arch PRIVATE nuttx_arch)
target_link_libraries(drivers_boards_common_arch PRIVATE arch_adc)
src/drivers/boards/common/stm32/board_hw_rev_ver.c
+1 -1
View File
@@ -38,7 +38,7 @@
*/
#include <drivers/drv_adc.h>
#include <px4_arch_adc.h>
#include <px4_arch/adc.h>
#include <px4_config.h>
#include <stdio.h>
#include "board_config.h"
src/drivers/drv_adc.h
-1
View File
@@ -40,7 +40,6 @@
#pragma once
#include <px4_arch_adc.h>
#include <stdint.h>
#include <sys/ioctl.h>
src/drivers/kinetis/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_library(drivers_arch
drv_io_timer.c
drv_pwm_servo.c
drv_pwm_trigger.c
drv_input_capture.c
)
add_dependencies(drivers_arch prebuild_targets)
target_link_libraries(drivers_arch PRIVATE drivers_board)
target_compile_options(drivers_arch PRIVATE -Wno-cast-align) # TODO: fix and enable
src/drivers/kinetis/drv_input_capture.c
-457
View File
@@ -1,457 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_input_capture.c
*
* Servo driver supporting input capture connected to STM32 timer blocks.
*
* Works with any of the 'generic' or 'advanced' STM32 timers that
* have input pins.
*
* Require an interrupt.
*
* The use of thie interface is mutually exclusive with the pwm
* because the same timers are used and there is a resource contention
* with the ARR as it sets the pwm rate and in this driver needs to match
* that of the hrt to back calculate the actual point in time the edge
* was detected.
*
* This is accomplished by taking the difference between the current
* count rCNT snapped at the time interrupt and the rCCRx captured on the
* edge transition. This delta is applied to hrt time and the resulting
* value is the absolute system time the edge occured.
*
*
*/
#include <px4_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_input_capture.h>
#include "drv_io_timer.h"
#include "drv_input_capture.h"
#include <kinetis.h>
#include "chip/kinetis_sim.h"
#include "chip/kinetis_ftm.h"
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
/* Timer register accessors */
#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET)
static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
capture_callback_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time, uint16_t isrs_rcnt,
uint16_t capture)
{
channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
}
channel_stats[chan_index].chan_in_edges_out++;
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
uint32_t overflow = _REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF;
if (overflow) {
/* Error we has a second edge before we cleared CCxR */
channel_stats[chan_index].overflows++;
}
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index,
channel_stats[chan_index].last_time,
channel_stats[chan_index].last_edge, overflow);
}
}
static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context)
{
irqstate_t flags = px4_enter_critical_section();
channel_handlers[channel].callback = callback;
channel_handlers[channel].context = context;
px4_leave_critical_section(flags);
}
static void input_capture_unbind(unsigned channel)
{
input_capture_bind(channel, NULL, NULL);
}
int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
capture_callback_t callback, void *context)
{
if (filter > FTM_FILTER_CH0FVAL_MASK) {
return -EINVAL;
}
if (edge > Both) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
/* This register selects the filter value for the inputs of channels.
Channels 4, 5, 6 and 7 do not have an input filter.
*/
if (filter && timer_io_channels[channel].timer_channel - 1 > 3) {
return -EINVAL;
}
if (edge == Disabled) {
io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel);
input_capture_unbind(channel);
} else {
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
input_capture_bind(channel, callback, context);
rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context);
if (rv != 0) {
return rv;
}
rv = up_input_capture_set_filter(channel, filter);
if (rv == 0) {
rv = up_input_capture_set_trigger(channel, edge);
if (rv == 0) {
rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel);
}
}
}
}
return rv;
}
int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -EINVAL;
if (timer_io_channels[channel].timer_channel - 1 <= 3) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
rv = OK;
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rFILTER(timer) & FTM_FILTER_CH0FVAL_MASK;
*filter = (rvalue >> FTM_FILTER_CH0FVAL_SHIFT);
break;
case 2:
rvalue = rFILTER(timer) & FTM_FILTER_CH1FVAL_MASK;
*filter = (rvalue >> FTM_FILTER_CH1FVAL_SHIFT);
break;
case 3:
rvalue = rFILTER(timer) & FTM_FILTER_CH2FVAL_MASK;
*filter = (rvalue >> FTM_FILTER_CH2FVAL_SHIFT);
break;
case 4:
rvalue = rFILTER(timer) & FTM_FILTER_CH3FVAL_MASK;
*filter = (rvalue >> FTM_FILTER_CH3FVAL_SHIFT);
break;
default:
rv = -EIO;
}
}
}
}
return rv;
}
int up_input_capture_set_filter(unsigned channel, capture_filter_t filter)
{
if (filter > FTM_FILTER_CH0FVAL_MASK) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
irqstate_t flags = px4_enter_critical_section();
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rFILTER(timer) & ~FTM_FILTER_CH0FVAL_MASK;
rvalue |= (filter << FTM_FILTER_CH0FVAL_SHIFT);
rFILTER(timer) = rvalue;
break;
case 2:
rvalue = rFILTER(timer) & ~FTM_FILTER_CH1FVAL_MASK;
rvalue |= (filter << FTM_FILTER_CH1FVAL_SHIFT);
rFILTER(timer) = rvalue;
break;
case 3:
rvalue = rFILTER(timer) & ~FTM_FILTER_CH2FVAL_MASK;
rvalue |= (filter << FTM_FILTER_CH2FVAL_SHIFT);
rFILTER(timer) = rvalue;
break;
case 4:
rvalue = rFILTER(timer) & ~FTM_FILTER_CH2FVAL_MASK;
rvalue |= (filter << FTM_FILTER_CH2FVAL_SHIFT);
rFILTER(timer) = rvalue;
break;
default:
rv = -EIO;
}
px4_leave_critical_section(flags);
}
}
return rv;
}
int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue = _REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1));
rvalue &= (FTM_CSC_MSB | FTM_CSC_MSA);
switch (rvalue) {
case (FTM_CSC_MSA):
*edge = Rising;
break;
case (FTM_CSC_MSB):
*edge = Falling;
break;
case (FTM_CSC_MSB|FTM_CSC_MSA):
*edge = Both;
break;
default:
rv = -EIO;
}
}
}
return rv;
}
int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint16_t edge_bits = 0;
switch (edge) {
case Disabled:
break;
case Rising:
edge_bits = FTM_CSC_MSA;
break;
case Falling:
edge_bits = FTM_CSC_MSB;
break;
case Both:
edge_bits = (FTM_CSC_MSB | FTM_CSC_MSA);
break;
default:
return -EINVAL;;
}
uint32_t timer = timer_io_channels[channel].timer_index;
irqstate_t flags = px4_enter_critical_section();
uint32_t rvalue = _REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1));
rvalue &= (FTM_CSC_MSB | FTM_CSC_MSA);
rvalue |= edge_bits;
_REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1)) = rvalue;
px4_leave_critical_section(flags);
rv = OK;
}
}
return rv;
}
int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
irqstate_t flags = px4_enter_critical_section();
*callback = channel_handlers[channel].callback;
*context = channel_handlers[channel].context;
px4_leave_critical_section(flags);
rv = OK;
}
}
return rv;
}
int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
input_capture_bind(channel, callback, context);
rv = 0;
}
}
return rv;
}
int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
irqstate_t flags = px4_enter_critical_section();
*stats = channel_stats[channel];
if (clear) {
memset(&channel_stats[channel], 0, sizeof(*stats));
}
px4_leave_critical_section(flags);
}
return rv;
}
src/drivers/kinetis/drv_input_capture.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_input_capture.h
*
* stm32-specific input capture data.
*/
#pragma once
#include <drivers/drv_input_capture.h>
src/drivers/kinetis/drv_io_timer.c
-891
View File
@@ -1,891 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_io_timer.c
*
* Servo driver supporting PWM servos connected to Kinetis FTM timer blocks.
*/
#include <px4_config.h>
#include <systemlib/px4_macros.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_io_timer.h"
#include <kinetis.h>
#include "chip/kinetis_sim.h"
#include "chip/kinetis_ftm.h"
/* The FTM pre-scalers are limited to Divide by 2^n where n={1-7}
* Therefore we use Y1 at 16 Mhz to drive FTM_CLKIN0 (PCT12)
* and use this at a 16Mhz clock for FTM0, FTM2 and FTM3.
*
* FTM0 will drive FMU_CH1-4, FTM3 will drive FMU_CH5,6, and
* U_TRI. FTM2 will be used as input capture on U_ECH
*/
#if !defined(BOARD_PWM_FREQ)
#define BOARD_PWM_FREQ 1000000
#endif
#if !defined(BOARD_ONESHOT_FREQ)
#define BOARD_ONESHOT_FREQ 8000000
#endif
#define FTM_SRC_CLOCK_FREQ 16000000
#define MAX_CHANNELS_PER_TIMER 8
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
/* Timer register accessors */
#define rSC(_tmr) REG(_tmr,KINETIS_FTM_SC_OFFSET)
#define rCNT(_tmr) REG(_tmr,KINETIS_FTM_CNT_OFFSET)
#define rMOD(_tmr) REG(_tmr,KINETIS_FTM_MOD_OFFSET)
#define rC0SC(_tmr) REG(_tmr,KINETIS_FTM_C0SC_OFFSET)
#define rC0V(_tmr) REG(_tmr,KINETIS_FTM_C0V_OFFSET)
#define rC1SC(_tmr) REG(_tmr,KINETIS_FTM_C1SC_OFFSET)
#define rC1V(_tmr) REG(_tmr,KINETIS_FTM_C1V_OFFSET)
#define rC2SC(_tmr) REG(_tmr,KINETIS_FTM_C2SC_OFFSET)
#define rC2V(_tmr) REG(_tmr,KINETIS_FTM_C2V_OFFSET)
#define rC3SC(_tmr) REG(_tmr,KINETIS_FTM_C3SC_OFFSET)
#define rC3V(_tmr) REG(_tmr,KINETIS_FTM_C3V_OFFSET)
#define rC4SC(_tmr) REG(_tmr,KINETIS_FTM_C4SC_OFFSET)
#define rC4V(_tmr) REG(_tmr,KINETIS_FTM_C4V_OFFSET)
#define rC5SC(_tmr) REG(_tmr,KINETIS_FTM_C5SC_OFFSET)
#define rC5V(_tmr) REG(_tmr,KINETIS_FTM_C5V_OFFSET)
#define rC6SC(_tmr) REG(_tmr,KINETIS_FTM_C6SC_OFFSET)
#define rC6V(_tmr) REG(_tmr,KINETIS_FTM_C6V_OFFSET)
#define rC7SC(_tmr) REG(_tmr,KINETIS_FTM_C7SC_OFFSET)
#define rC7V(_tmr) REG(_tmr,KINETIS_FTM_C7V_OFFSET)
#define rCNTIN(_tmr) REG(_tmr,KINETIS_FTM_CNTIN_OFFSET)
#define rSTATUS(_tmr) REG(_tmr,KINETIS_FTM_STATUS_OFFSET)
#define rMODE(_tmr) REG(_tmr,KINETIS_FTM_MODE_OFFSET)
#define rSYNC(_tmr) REG(_tmr,KINETIS_FTM_SYNC_OFFSET)
#define rOUTINIT(_tmr) REG(_tmr,KINETIS_FTM_OUTINIT_OFFSET)
#define rOUTMASK(_tmr) REG(_tmr,KINETIS_FTM_OUTMASK_OFFSET)
#define rCOMBINE(_tmr) REG(_tmr,KINETIS_FTM_COMBINE_OFFSET)
#define rDEADTIME(_tmr) REG(_tmr,KINETIS_FTM_DEADTIME_OFFSET)
#define rEXTTRIG(_tmr) REG(_tmr,KINETIS_FTM_EXTTRIG_OFFSET)
#define rPOL(_tmr) REG(_tmr,KINETIS_FTM_POL_OFFSET)
#define rFMS(_tmr) REG(_tmr,KINETIS_FTM_FMS_OFFSET)
#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET)
#define rFLTCTRL(_tmr) REG(_tmr,KINETIS_FTM_FLTCTRL_OFFSET)
#define rQDCTRL(_tmr) REG(_tmr,KINETIS_FTM_QDCTRL_OFFSET)
#define rCONF(_tmr) REG(_tmr,KINETIS_FTM_CONF_OFFSET)
#define rFLTPOL(_tmr) REG(_tmr,KINETIS_FTM_FLTPOL_OFFSET)
#define rSYNCONF(_tmr) REG(_tmr,KINETIS_FTM_SYNCONF_OFFSET)
#define rINVCTRL(_tmr) REG(_tmr,KINETIS_FTM_INVCTRL_OFFSET)
#define rSWOCTRL(_tmr) REG(_tmr,KINETIS_FTM_SWOCTRL_OFFSET)
#define rPWMLOAD(_tmr) REG(_tmr,KINETIS_FTM_PWMLOAD_OFFSET)
#define CnSC_RESET (FTM_CSC_CHF|FTM_CSC_CHIE|FTM_CSC_MSB|FTM_CSC_MSA|FTM_CSC_ELSB|FTM_CSC_ELSA|FTM_CSC_DMA)
#define CnSC_CAPTURE_INIT (FTM_CSC_CHIE|FTM_CSC_ELSB|FTM_CSC_ELSA) // Both
#if defined(BOARD_PWM_DRIVE_ACTIVE_LOW)
#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSA)
#else
#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSB)
#endif
#define FTM_SYNC (FTM_SYNC_SWSYNC)
#define CnSC_PWMIN_INIT 0 // TBD
// NotUsed PWMOut PWMIn Capture OneShot Trigger
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
static io_timer_allocation_t once = 0;
typedef struct channel_stat_t {
uint32_t isr_cout;
uint32_t overflows;
} channel_stat_t;
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
channel_handler_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
static int io_timer_handler(uint16_t timer_index)
{
/* Read the count at the time of the interrupt */
uint16_t count = rCNT(timer_index);
/* Read the HRT at the time of the interrupt */
hrt_abstime now = hrt_absolute_time();
const io_timers_t *tmr = &io_timers[timer_index];
/* What is pending */
uint32_t statusr = rSTATUS(timer_index);
/* Acknowledge all that are pending */
rSTATUS(timer_index) = 0;
/* Iterate over the timer_io_channels table */
for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
uint16_t chan = 1 << chan_index;
if (statusr & chan) {
io_timer_channel_stats[chan_index].isr_cout++;
/* Call the client to read the rCnV etc and clear the CHnF */
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr,
chan_index, &timer_io_channels[chan_index],
now, count, _REG32(tmr->base, KINETIS_FTM_CV_OFFSET(chan_index)));
}
}
/* Did it set again during call out ?*/
if (rSTATUS(timer_index) & chan) {
/* Error we has a second edge before we serviced the fist */
io_timer_channel_stats[chan_index].overflows++;
}
}
return 0;
}
int io_timer_handler0(int irq, void *context, void *arg)
{
return io_timer_handler(0);
}
int io_timer_handler1(int irq, void *context, void *arg)
{
return io_timer_handler(1);
}
int io_timer_handler2(int irq, void *context, void *arg)
{
return io_timer_handler(2);
}
int io_timer_handler3(int irq, void *context, void *arg)
{
return io_timer_handler(3);
}
static inline int validate_timer_index(unsigned timer)
{
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
}
static inline int is_timer_uninitalized(unsigned timer)
{
int rv = 0;
if (once & 1 << timer) {
rv = -EBUSY;
}
return rv;
}
static inline void set_timer_initalized(unsigned timer)
{
once |= 1 << timer;
}
static inline void set_timer_deinitalized(unsigned timer)
{
once &= ~(1 << timer);
}
static inline int channels_timer(unsigned channel)
{
return timer_io_channels[channel].timer_index;
}
static inline int get_timers_firstchannels(unsigned timer)
{
int channel = -1;
if (validate_timer_index(timer) == 0) {
channel = timer_io_channels[io_timers[timer].first_channel_index].timer_channel;
}
return channel;
}
static uint32_t get_timer_channels(unsigned timer)
{
uint32_t channels = 0;
static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
if (validate_timer_index(timer) < 0) {
return channels;
} else {
if (channels_cache[timer] == 0) {
const io_timers_t *tmr = &io_timers[timer];
/* Gather the channel bits that belong to the timer */
for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
channels |= 1 << chan_index;
}
/* cache them */
channels_cache[timer] = channels;
}
}
return channels_cache[timer];
}
static inline int is_channels_timer_uninitalized(unsigned channel)
{
return is_timer_uninitalized(channels_timer(channel));
}
int io_timer_is_channel_free(unsigned channel)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
rv = -EBUSY;
}
}
return rv;
}
int io_timer_validate_channel_index(unsigned channel)
{
int rv = -EINVAL;
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) {
unsigned timer = timer_io_channels[channel].timer_index;
/* test timer for validity */
if ((io_timers[timer].base != 0) &&
(timer_io_channels[channel].gpio_out != 0) &&
(timer_io_channels[channel].gpio_in != 0)) {
rv = 0;
}
}
return rv;
}
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
{
if (mode < IOTimerChanModeSize) {
return channel_allocations[mode];
}
return 0;
}
int io_timer_get_channel_mode(unsigned channel)
{
io_timer_channel_allocation_t bit = 1 << channel;
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
if (bit & channel_allocations[mode]) {
return mode;
}
}
return -1;
}
static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode,
io_timer_channel_mode_t new_mode)
{
/* If caller mode is not based on current setting adjust it */
if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) {
mode = IOTimerChanMode_NotUsed;
}
/* Remove old set of channels from original */
channel_allocations[mode] &= ~channels;
/* Will this change ?*/
uint32_t before = channel_allocations[new_mode] & channels;
/* add in the new set */
channel_allocations[new_mode] |= channels;
/* Indicate a mode change */
return before ^ channels;
}
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = io_timer_is_channel_free(channel);
if (rv == 0) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
channel_allocations[mode] |= bit;
}
return rv;
}
static inline int free_channel_resource(unsigned channel)
{
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[mode] &= ~bit;
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
}
return mode;
}
int io_timer_free_channel(unsigned channel)
{
if (io_timer_validate_channel_index(channel) != 0) {
return -EINVAL;
}
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_set_enable(false, mode, 1 << channel);
free_channel_resource(channel);
}
return 0;
}
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = -EINVAL;
if (mode != IOTimerChanMode_NotUsed) {
rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = allocate_channel_resource(channel, mode);
}
}
return rv;
}
static int timer_set_rate(unsigned timer, unsigned rate)
{
irqstate_t flags = px4_enter_critical_section();
uint32_t save = rSC(timer);
rSC(timer) = save & ~(FTM_SC_CLKS_MASK);
/* configure the timer to update at the desired rate */
rMOD(timer) = (BOARD_PWM_FREQ / rate) - 1;
rSC(timer) = save;
/* generate an update event; reloads the counter and all registers */
rSYNC(timer) = FTM_SYNC;
px4_leave_critical_section(flags);
return 0;
}
static inline uint32_t div2psc(int div)
{
return 31 - __builtin_clz(div);
}
static inline void io_timer_set_oneshot_mode(unsigned timer)
{
/* Ideally, we would want per channel One pulse mode in HW
* Alas The FTM anly support onshot capture)
* todo:We can do this in an ISR later
* But since we do not have that
* We try to get the longest rate we can.
* On 16 bit timers this is 4.68 Ms.
*/
irqstate_t flags = px4_enter_critical_section();
rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK);
rMOD(timer) = 0xffff;
rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / BOARD_ONESHOT_FREQ));
px4_leave_critical_section(flags);
}
static inline void io_timer_set_PWM_mode(unsigned timer)
{
irqstate_t flags = px4_enter_critical_section();
rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK);
rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / BOARD_PWM_FREQ));
px4_leave_critical_section(flags);
}
void io_timer_trigger(void)
{
int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);
uint32_t action_cache[MAX_IO_TIMERS] = {0};
int actions = 0;
/* Pre-calculate the list of timers to Trigger */
for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
if (validate_timer_index(timer) == 0) {
int channels = get_timer_channels(timer);
if (oneshots & channels) {
action_cache[actions++] = io_timers[timer].base;
}
}
}
/* Now do them all wit the shortest delay in between */
irqstate_t flags = px4_enter_critical_section();
for (actions = 0; action_cache[actions] != 0 && actions < MAX_IO_TIMERS; actions++) {
_REG32(action_cache[actions], KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
}
px4_leave_critical_section(flags);
}
int io_timer_init_timer(unsigned timer)
{
/* Do this only once per timer */
int rv = is_timer_uninitalized(timer);
if (rv == 0) {
irqstate_t flags = px4_enter_critical_section();
set_timer_initalized(timer);
/* enable the timer clock before we try to talk to it */
uint32_t regval = _REG(io_timers[timer].clock_register);
regval |= io_timers[timer].clock_bit;
_REG(io_timers[timer].clock_register) = regval;
/* disable and configure the timer */
rSC(timer) = FTM_SC_CLKS_NONE;
rCNT(timer) = 0;
rMODE(timer) = 0;
rSYNCONF(timer) = (FTM_SYNCONF_SYNCMODE | FTM_SYNCONF_SWWRBUF | FTM_SYNCONF_SWRSTCNT);
/* Set to run in debug mode */
rCONF(timer) |= FTM_CONF_BDMMODE_MASK;
/* enable the timer */
io_timer_set_PWM_mode(timer);
/*
* Note we do the Standard PWM Out init here
* default to updating at 50Hz
*/
timer_set_rate(timer, 50);
/*
* Note that the timer is left disabled with IRQ subs installed
* and active but DEIR bits are not set.
*/
irq_attach(io_timers[timer].vectorno, io_timers[timer].handler, NULL);
up_enable_irq(io_timers[timer].vectorno);
px4_leave_critical_section(flags);
}
return rv;
}
int io_timer_set_rate(unsigned timer, unsigned rate)
{
int rv = EBUSY;
/* Get the channel bits that belong to the timer */
uint32_t channels = get_timer_channels(timer);
/* Check that all channels are either in PWM or Oneshot */
if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
channel_allocations[IOTimerChanMode_OneShot] |
channel_allocations[IOTimerChanMode_NotUsed])) ==
channels) {
/* Change only a timer that is owned by pwm or one shot */
/* Request to use OneShot ?*/
if (rate == 0) {
/* Request to use OneShot
*
* We are here because ALL these channels were either PWM or Oneshot
* Now they need to be Oneshot
*/
/* Did the allocation change */
if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
io_timer_set_oneshot_mode(timer);
}
} else {
/* Request to use PWM
*
* We are here because ALL these channels were either PWM or Oneshot
* Now they need to be PWM
*/
if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
io_timer_set_PWM_mode(timer);
}
timer_set_rate(timer, rate);
}
rv = OK;
}
return rv;
}
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context)
{
uint32_t gpio = timer_io_channels[channel].gpio_in;
uint32_t clearbits = CnSC_RESET;
uint32_t setbits = CnSC_CAPTURE_INIT;
/* figure out the GPIO config first */
switch (mode) {
case IOTimerChanMode_OneShot:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_Trigger:
setbits = CnSC_PWMOUT_INIT;
gpio = 0;
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
break;
case IOTimerChanMode_NotUsed:
setbits = 0;
break;
default:
return -EINVAL;
}
int rv = allocate_channel(channel, mode);
/* Valid channel should now be reserved in new mode */
if (rv >= 0) {
/* Blindly try to initialize the timer - it will only do it once */
io_timer_init_timer(channels_timer(channel));
irqstate_t flags = px4_enter_critical_section();
/* Set up IO */
if (gpio) {
px4_arch_configgpio(gpio);
}
unsigned timer = channels_timer(channel);
/* configure the channel */
uint32_t chan = timer_io_channels[channel].timer_channel - 1;
uint16_t rvalue = REG(timer, KINETIS_FTM_CSC_OFFSET(chan));
rvalue &= ~clearbits;
rvalue |= setbits;
REG(timer, KINETIS_FTM_CSC_OFFSET(chan)) = rvalue;
//if (gpio){ TODO: NEEDED?
REG(timer, KINETIS_FTM_CV_OFFSET(0)) = 0;
channel_handlers[channel].callback = channel_handler;
channel_handlers[channel].context = context;
px4_leave_critical_section(flags);
}
return rv;
}
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
{
typedef struct action_cache_rp_t {
uint32_t cnsc_offset;
uint32_t cnsc_value;
uint32_t gpio;
} action_cache_rp_t;
struct action_cache_t {
uint32_t base;
uint32_t index;
action_cache_rp_t cnsc[MAX_CHANNELS_PER_TIMER];
} action_cache[MAX_IO_TIMERS];
memset(action_cache, 0, sizeof(action_cache));
uint32_t bits = state ? CnSC_PWMOUT_INIT : 0;
switch (mode) {
case IOTimerChanMode_NotUsed:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_OneShot:
case IOTimerChanMode_Trigger:
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
if (state) {
bits |= FTM_CSC_CHIE;
}
break;
default:
return -EINVAL;
}
/* Was the request for all channels in this mode ?*/
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
/* Yes - we provide them */
masks = channel_allocations[mode];
} else {
/* No - caller provided mask */
/* Only allow the channels in that mode to be affected */
masks &= channel_allocations[mode];
}
/* Pre calculate all the changes */
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
if (masks & (1 << chan_index)) {
masks &= ~(1 << chan_index);
if (io_timer_validate_channel_index(chan_index) == 0) {
uint32_t chan = timer_io_channels[chan_index].timer_channel - 1;
uint32_t timer = channels_timer(chan_index);
action_cache[timer].base = io_timers[timer].base;
action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + KINETIS_FTM_CSC_OFFSET(chan);
action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits;
if ((state &&
(mode == IOTimerChanMode_PWMOut ||
mode == IOTimerChanMode_OneShot ||
mode == IOTimerChanMode_Trigger))) {
action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out;
}
action_cache[timer].index++;
}
}
}
irqstate_t flags = px4_enter_critical_section();
for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
uint32_t any_on = 0;
if (action_cache[actions].base != 0) {
for (unsigned int index = 0; index < action_cache[actions].index; index++) {
if (action_cache[actions].cnsc[index].gpio) {
px4_arch_configgpio(action_cache[actions].cnsc[index].gpio);
}
_REG(action_cache[actions].cnsc[index].cnsc_offset) = action_cache[actions].cnsc[index].cnsc_value;
any_on |= action_cache[actions].cnsc[index].cnsc_value;
}
/* Any On ?*/
/* Assume not */
uint32_t regval = _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET);
regval &= ~(FTM_SC_CLKS_MASK);
if (any_on != 0) {
/* force an update to preload all registers */
_REG32(action_cache[actions].base, KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
/* arm requires the timer be enabled */
regval |= (FTM_SC_CLKS_EXTCLK);
}
_REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET) = regval;
}
}
px4_leave_critical_section(flags);
return 0;
}
int io_timer_set_ccr(unsigned channel, uint16_t value)
{
int rv = io_timer_validate_channel_index(channel);
int mode = io_timer_get_channel_mode(channel);
if (rv == 0) {
if ((mode != IOTimerChanMode_PWMOut) &&
(mode != IOTimerChanMode_OneShot) &&
(mode != IOTimerChanMode_Trigger)) {
rv = -EIO;
} else {
/* configure the channel */
/* todo:This is a HACK!
* Was getting:
* servo 0 readback error, got 1001 expected 1000
* None of the SW syncs seamed update the CV
* So we stop the counter to get an immediate update.
*/
uint32_t timer = channels_timer(channel);
irqstate_t flags = px4_enter_critical_section();
uint32_t save = rSC(timer);
rSC(timer) = save & ~(FTM_SC_CLKS_MASK);
REG(timer, KINETIS_FTM_CV_OFFSET(timer_io_channels[channel].timer_channel - 1)) = value;
rSC(timer) = save;
px4_leave_critical_section(flags);
}
}
return rv;
}
uint16_t io_channel_get_ccr(unsigned channel)
{
uint16_t value = 0;
if (io_timer_validate_channel_index(channel) == 0) {
int mode = io_timer_get_channel_mode(channel);
if ((mode == IOTimerChanMode_PWMOut) ||
(mode == IOTimerChanMode_OneShot) ||
(mode == IOTimerChanMode_Trigger)) {
value = REG(channels_timer(channel), KINETIS_FTM_CV_OFFSET(timer_io_channels[channel].timer_channel - 1));
}
}
return value;
}
uint32_t io_timer_get_group(unsigned timer)
{
return get_timer_channels(timer);
}
src/drivers/kinetis/drv_io_timer.h
-133
View File
@@ -1,133 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_io_timer.h
*
* stm32-specific PWM output data.
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <drivers/drv_hrt.h>
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 4
#define MAX_TIMER_IO_CHANNELS 16
#define MAX_LED_TIMERS 2
#define MAX_TIMER_LED_CHANNELS 6
#define IO_TIMER_ALL_MODES_CHANNELS 0
typedef enum io_timer_channel_mode_t {
IOTimerChanMode_NotUsed = 0,
IOTimerChanMode_PWMOut = 1,
IOTimerChanMode_PWMIn = 2,
IOTimerChanMode_Capture = 3,
IOTimerChanMode_OneShot = 4,
IOTimerChanMode_Trigger = 5,
IOTimerChanModeSize
} io_timer_channel_mode_t;
typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
/* array of timers dedicated to PWM in and out and capture use
*** Note that the clock_freq is set to the source in the clock tree that
*** feeds this specific timer. This can differs by Timer!
*** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz
*** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz
*** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz
*** the resulting PSC will be one and the timer will count at it's clock frequency.
*/
typedef struct io_timers_t {
uint32_t base; /* Base address of the timer */
uint32_t clock_register; /* SIM_SCGCn */
uint32_t clock_bit; /* SIM_SCGCn bit pos */
uint32_t vectorno; /* IRQ number */
uint32_t first_channel_index; /* 0 based index in timer_io_channels */
uint32_t last_channel_index; /* 0 based index in timer_io_channels */
xcpt_t handler;
} io_timers_t;
/* array of channels in logical order */
typedef struct timer_io_channels_t {
uint32_t gpio_out; /* The timer channel GPIO for PWM */
uint32_t gpio_in; /* The timer channel GPIO for Capture */
uint8_t timer_index; /* 0 based index in the io_timers_t table */
uint8_t timer_channel; /* 1 based channel index GPIO_FTMt_CHcIN = c+1) */
} timer_io_channels_t;
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time, uint16_t isrs_rcnt,
uint16_t capture);
/* supplied by board-specific code */
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
__EXPORT int io_timer_handler0(int irq, void *context, void *arg);
__EXPORT int io_timer_handler1(int irq, void *context, void *arg);
__EXPORT int io_timer_handler2(int irq, void *context, void *arg);
__EXPORT int io_timer_handler3(int irq, void *context, void *arg);
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context);
__EXPORT int io_timer_init_timer(unsigned timer);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
io_timer_channel_allocation_t masks);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
__EXPORT uint32_t io_timer_get_group(unsigned timer);
__EXPORT int io_timer_validate_channel_index(unsigned channel);
__EXPORT int io_timer_is_channel_free(unsigned channel);
__EXPORT int io_timer_free_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__EXPORT extern void io_timer_trigger(void);
__END_DECLS
src/drivers/kinetis/drv_pwm_servo.c
-163
View File
@@ -1,163 +0,0 @@
/****************************************************************************
*
* 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 <px4_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_io_timer.h"
#include "drv_pwm_servo.h"
#include <kinetis.h>
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
return io_timer_set_ccr(channel, value);
}
servo_position_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}
int up_pwm_servo_init(uint32_t channel_mask)
{
/* Init channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
// First free the current set of PWMs
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (current & (1 << channel)) {
io_timer_free_channel(channel);
current &= ~(1 << channel);
}
}
// Now allocate the new set
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
// First free any that were not PWM mode before
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
channel_mask &= ~(1 << channel);
}
}
return OK;
}
void up_pwm_servo_deinit(void)
{
/* disable the timers */
up_pwm_servo_arm(false);
}
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) {
return ERROR;
}
/* Allow a rate of 0 to enter oneshot mode */
if (rate != 0) {
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1) {
return -ERANGE;
}
if (rate > 10000) {
return -ERANGE;
}
}
return io_timer_set_rate(group, rate);
}
void up_pwm_update(void)
{
io_timer_trigger();
}
int up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
up_pwm_servo_set_rate_group_update(i, rate);
}
return 0;
}
uint32_t up_pwm_servo_get_rate_group(unsigned group)
{
return io_timer_get_group(group);
}
void
up_pwm_servo_arm(bool armed)
{
io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
}
src/drivers/kinetis/drv_pwm_servo.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* 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>
src/drivers/kinetis/drv_pwm_trigger.c
-108
View File
@@ -1,108 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_pwm_trigger.c
*
*/
#include <px4_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_trigger.h>
#include "drv_io_timer.h"
#include "drv_pwm_trigger.h"
int up_pwm_trigger_set(unsigned channel, uint16_t value)
{
return io_timer_set_ccr(channel, value);
}
int up_pwm_trigger_init(uint32_t channel_mask)
{
/* Init channels */
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
// First free any that were not trigger mode before
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
channel_mask &= ~(1 << channel);
}
}
/* Enable the timers */
up_pwm_trigger_arm(true);
return OK;
}
void up_pwm_trigger_deinit()
{
/* Disable the timers */
up_pwm_trigger_arm(false);
/* Deinit channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_Trigger);
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (current & (1 << channel)) {
io_timer_channel_init(channel, IOTimerChanMode_NotUsed, NULL, NULL);
current &= ~(1 << channel);
}
}
}
void
up_pwm_trigger_arm(bool armed)
{
io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS);
}
src/drivers/kinetis/drv_pwm_trigger.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_pwm_trigger.h
*
* stm32-specific PWM output data.
*/
#pragma once
#include <drivers/drv_pwm_trigger.h>
src/drivers/lights/rgbled_pwm/CMakeLists.txt
+1
View File
@@ -40,5 +40,6 @@ px4_add_module(
rgbled_pwm.cpp
DEPENDS
led
arch_io_pins
arch_led_pwm
)
src/drivers/px4fmu/CMakeLists.txt
+1
View File
@@ -38,6 +38,7 @@ px4_add_module(
SRCS
fmu.cpp
DEPENDS
arch_io_pins
circuit_breaker
mixer
pwm_limit
src/drivers/px4io/px4io_serial.cpp
+1 -1
View File
@@ -39,7 +39,7 @@
#include "px4io_driver.h"
#include <px4io_serial/arch_px4io_serial.h>
#include <px4_arch/px4io_serial.h>
static PX4IO_serial *g_interface;
src/drivers/stm32/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_library(drivers_arch
drv_io_timer.c
drv_pwm_servo.c
drv_pwm_trigger.c
drv_input_capture.c
)
add_dependencies(drivers_arch prebuild_targets)
target_link_libraries(drivers_arch PRIVATE drivers_board)
target_compile_options(drivers_arch PRIVATE -Wno-cast-align) # TODO: fix and enable
src/drivers/stm32/drv_input_capture.c
-506
View File
@@ -1,506 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_input_capture.c
*
* Servo driver supporting input capture connected to STM32 timer blocks.
*
* Works with any of the 'generic' or 'advanced' STM32 timers that
* have input pins.
*
* Require an interrupt.
*
* The use of thie interface is mutually exclusive with the pwm
* because the same timers are used and there is a resource contention
* with the ARR as it sets the pwm rate and in this driver needs to match
* that of the hrt to back calculate the actual point in time the edge
* was detected.
*
* This is accomplished by taking the difference between the current
* count rCNT snapped at the time interrupt and the rCCRx captured on the
* edge transition. This delta is applied to hrt time and the resulting
* value is the absolute system time the edge occured.
*
*
*/
#include <px4_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_input_capture.h>
#include "drv_io_timer.h"
#include "drv_input_capture.h"
#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)
#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 GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
capture_callback_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time, uint16_t isrs_rcnt)
{
uint16_t capture = _REG32(timer->base, chan->ccr_offset);
channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
}
channel_stats[chan_index].chan_in_edges_out++;
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
uint32_t overflow = _REG32(timer->base, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF;
if (overflow) {
/* Error we has a second edge before we cleared CCxR */
channel_stats[chan_index].overflows++;
}
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index,
channel_stats[chan_index].last_time,
channel_stats[chan_index].last_edge, overflow);
}
}
static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context)
{
irqstate_t flags = px4_enter_critical_section();
channel_handlers[channel].callback = callback;
channel_handlers[channel].context = context;
px4_leave_critical_section(flags);
}
static void input_capture_unbind(unsigned channel)
{
input_capture_bind(channel, NULL, NULL);
}
int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
capture_callback_t callback, void *context)
{
if (filter > GTIM_CCMR1_IC1F_MASK) {
return -EINVAL;
}
if (edge > Both) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (edge == Disabled) {
io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel);
input_capture_unbind(channel);
} else {
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
input_capture_bind(channel, callback, context);
rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context);
if (rv != 0) {
return rv;
}
rv = up_input_capture_set_filter(channel, filter);
if (rv == 0) {
rv = up_input_capture_set_trigger(channel, edge);
if (rv == 0) {
rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel);
}
}
}
}
return rv;
}
int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
rv = OK;
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCMR1(timer) & GTIM_CCMR1_IC1F_MASK;
*filter = (rvalue >> GTIM_CCMR1_IC1F_SHIFT);
break;
case 2:
rvalue = rCCMR1(timer) & GTIM_CCMR1_IC2F_MASK;
*filter = (rvalue >> GTIM_CCMR1_IC2F_SHIFT);
break;
case 3:
rvalue = rCCMR2(timer) & GTIM_CCMR2_IC3F_MASK;
*filter = (rvalue >> GTIM_CCMR2_IC3F_SHIFT);
break;
case 4:
rvalue = rCCMR2(timer) & GTIM_CCMR2_IC4F_MASK;
*filter = (rvalue >> GTIM_CCMR2_IC4F_SHIFT);
break;
default:
rv = -EIO;
}
}
}
return rv;
}
int up_input_capture_set_filter(unsigned channel, capture_filter_t filter)
{
if (filter > GTIM_CCMR1_IC1F_MASK) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
irqstate_t flags = px4_enter_critical_section();
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC1F_MASK;
rvalue |= (filter << GTIM_CCMR1_IC1F_SHIFT);
rCCMR1(timer) = rvalue;
break;
case 2:
rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC2F_MASK;
rvalue |= (filter << GTIM_CCMR1_IC2F_SHIFT);
rCCMR1(timer) = rvalue;
break;
case 3:
rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC3F_MASK;
rvalue |= (filter << GTIM_CCMR2_IC3F_SHIFT);
rCCMR2(timer) = rvalue;
break;
case 4:
rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC4F_MASK;
rvalue |= (filter << GTIM_CCMR2_IC4F_SHIFT);
rCCMR2(timer) = rvalue;
break;
default:
rv = -EIO;
}
px4_leave_critical_section(flags);
}
}
return rv;
}
int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCER(timer) & (GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
break;
case 2:
rvalue = rCCER(timer) & (GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
rvalue >>= 4;
break;
case 3:
rvalue = rCCER(timer) & (GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
rvalue >>= 8;
break;
case 4:
rvalue = rCCER(timer) & (GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
rvalue >>= 12;
break;
default:
rv = -EIO;
}
if (rv == 0) {
switch (rvalue) {
case 0:
*edge = Rising;
break;
case (GTIM_CCER_CC1P | GTIM_CCER_CC1NP):
*edge = Both;
break;
case (GTIM_CCER_CC1P):
*edge = Falling;
break;
default:
rv = -EIO;
}
}
}
}
return rv;
}
int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint16_t edge_bits = 0xffff;
switch (edge) {
case Disabled:
break;
case Rising:
edge_bits = 0;
break;
case Falling:
edge_bits = GTIM_CCER_CC1P;
break;
case Both:
edge_bits = GTIM_CCER_CC1P | GTIM_CCER_CC1NP;
break;
default:
return -EINVAL;
}
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
rv = OK;
irqstate_t flags = px4_enter_critical_section();
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
rvalue |= edge_bits;
rCCER(timer) = rvalue;
break;
case 2:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
rvalue |= (edge_bits << 4);
rCCER(timer) = rvalue;
break;
case 3:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
rvalue |= edge_bits << 8;
rCCER(timer) = rvalue;
break;
case 4:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
rvalue |= edge_bits << 12;
rCCER(timer) = rvalue;
break;
default:
rv = -EIO;
}
px4_leave_critical_section(flags);
}
}
return rv;
}
int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
irqstate_t flags = px4_enter_critical_section();
*callback = channel_handlers[channel].callback;
*context = channel_handlers[channel].context;
px4_leave_critical_section(flags);
rv = OK;
}
}
return rv;
}
int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
input_capture_bind(channel, callback, context);
rv = 0;
}
}
return rv;
}
int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
irqstate_t flags = px4_enter_critical_section();
*stats = channel_stats[channel];
if (clear) {
memset(&channel_stats[channel], 0, sizeof(*stats));
}
px4_leave_critical_section(flags);
}
return rv;
}
#endif // defined(BOARD_HAS_CAPTURE)
src/drivers/stm32/drv_input_capture.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_input_capture.h
*
* stm32-specific input capture data.
*/
#pragma once
#include <drivers/drv_input_capture.h>
src/drivers/stm32/drv_io_timer.c
-916
View File
@@ -1,916 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_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 <px4_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_io_timer.h"
#include <stm32_gpio.h>
#include <stm32_tim.h>
#if defined(HAVE_GTIM_CCXNP)
#define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP
#else
# define HW_GTIM_CCER_CC1NP 0
#endif
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
/* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN
* then configure the timer to free-run at 1MHz.
* Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
* For instance .clock_freq = 1000000 would set the prescaler to 1.
* We also allow for overrides here but all timer register usage need to be
* taken into account
*/
#if !defined(BOARD_PWM_FREQ)
#define BOARD_PWM_FREQ 1000000
#endif
#if !defined(BOARD_ONESHOT_FREQ)
#define BOARD_ONESHOT_FREQ 8000000
#endif
#define MAX_CHANNELS_PER_TIMER 4
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_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)
#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF)
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
#define CCMR_C1_RESET 0x00ff
#define CCMR_C1_NUM_BITS 8
#define CCER_C1_NUM_BITS 4
#define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \
(GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \
(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_PWMIN_INIT 0 // TBD
#if defined(BOARD_PWM_DRIVE_ACTIVE_LOW)
#define CCER_C1_INIT (GTIM_CCER_CC1P | GTIM_CCER_CC1E)
#else
#define CCER_C1_INIT GTIM_CCER_CC1E
#endif
// NotUsed PWMOut PWMIn Capture OneShot Trigger
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0, 0, 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
static io_timer_allocation_t once = 0;
#if defined(BOARD_HAS_CAPTURE)
/* Stats and handlers are only useful for Capture */
typedef struct channel_stat_t {
uint32_t isr_cout;
uint32_t overflows;
} channel_stat_t;
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
channel_handler_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
#endif // defined(BOARD_HAS_CAPTURE)
static int io_timer_handler(uint16_t timer_index)
{
#if defined(BOARD_HAS_CAPTURE)
/* Read the count at the time of the interrupt */
uint16_t count = rCNT(timer_index);
/* Read the HRT at the time of the interrupt */
hrt_abstime now = hrt_absolute_time();
const io_timers_t *tmr = &io_timers[timer_index];
/* What is pending and enabled? */
uint16_t statusr = rSR(timer_index);
uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF;
/* Iterate over the timer_io_channels table */
for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
uint16_t masks = timer_io_channels[chan_index].masks;
/* Do we have an enabled channel */
if (enabled & masks) {
if (statusr & masks & GTIM_SR_CCIF) {
io_timer_channel_stats[chan_index].isr_cout++;
/* Call the client to read the CCxR etc and clear the CCxIF */
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr,
chan_index, &timer_io_channels[chan_index],
now, count);
}
}
if (statusr & masks & GTIM_SR_CCOF) {
/* Error we has a second edge before we cleared CCxR */
io_timer_channel_stats[chan_index].overflows++;
}
}
}
/* Clear all the SR bits for interrupt enabled channels only */
rSR(timer_index) = ~(statusr & (enabled | enabled << 8));
#endif // defined(BOARD_HAS_CAPTURE)
return 0;
}
int io_timer_handler0(int irq, void *context, void *arg)
{
return io_timer_handler(0);
}
int io_timer_handler1(int irq, void *context, void *arg)
{
return io_timer_handler(1);
}
int io_timer_handler2(int irq, void *context, void *arg)
{
return io_timer_handler(2);
}
int io_timer_handler3(int irq, void *context, void *arg)
{
return io_timer_handler(3);
}
int io_timer_handler4(int irq, void *context, void *arg)
{
return io_timer_handler(4);
}
static inline int validate_timer_index(unsigned timer)
{
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
}
static inline int is_timer_uninitalized(unsigned timer)
{
int rv = 0;
if (once & 1 << timer) {
rv = -EBUSY;
}
return rv;
}
static inline void set_timer_initalized(unsigned timer)
{
once |= 1 << timer;
}
static inline void set_timer_deinitalized(unsigned timer)
{
once &= ~(1 << timer);
}
static inline int channels_timer(unsigned channel)
{
return timer_io_channels[channel].timer_index;
}
static inline int get_timers_firstchannels(unsigned timer)
{
int channel = -1;
if (validate_timer_index(timer) == 0) {
channel = timer_io_channels[io_timers[timer].first_channel_index].timer_channel;
}
return channel;
}
static uint32_t get_timer_channels(unsigned timer)
{
uint32_t channels = 0;
static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
if (validate_timer_index(timer) < 0) {
return channels;
} else {
if (channels_cache[timer] == 0) {
const io_timers_t *tmr = &io_timers[timer];
/* Gather the channel bits that belong to the timer */
for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
channels |= 1 << chan_index;
}
/* cache them */
channels_cache[timer] = channels;
}
}
return channels_cache[timer];
}
static inline int is_channels_timer_uninitalized(unsigned channel)
{
return is_timer_uninitalized(channels_timer(channel));
}
int io_timer_is_channel_free(unsigned channel)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
rv = -EBUSY;
}
}
return rv;
}
int io_timer_validate_channel_index(unsigned channel)
{
int rv = -EINVAL;
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) {
unsigned timer = timer_io_channels[channel].timer_index;
/* test timer for validity */
if ((io_timers[timer].base != 0) &&
(timer_io_channels[channel].gpio_out != 0) &&
(timer_io_channels[channel].gpio_in != 0)) {
rv = 0;
}
}
return rv;
}
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
{
if (mode < IOTimerChanModeSize) {
return channel_allocations[mode];
}
return 0;
}
int io_timer_get_channel_mode(unsigned channel)
{
io_timer_channel_allocation_t bit = 1 << channel;
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
if (bit & channel_allocations[mode]) {
return mode;
}
}
return -1;
}
static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode,
io_timer_channel_mode_t new_mode)
{
/* If caller mode is not based on current setting adjust it */
if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) {
mode = IOTimerChanMode_NotUsed;
}
/* Remove old set of channels from original */
channel_allocations[mode] &= ~channels;
/* Will this change ?*/
uint32_t before = channel_allocations[new_mode] & channels;
/* add in the new set */
channel_allocations[new_mode] |= channels;
/* Indicate a mode change */
return before ^ channels;
}
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = io_timer_is_channel_free(channel);
if (rv == 0) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
channel_allocations[mode] |= bit;
}
return rv;
}
static inline int free_channel_resource(unsigned channel)
{
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[mode] &= ~bit;
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
}
return mode;
}
int io_timer_free_channel(unsigned channel)
{
if (io_timer_validate_channel_index(channel) != 0) {
return -EINVAL;
}
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_set_enable(false, mode, 1 << channel);
free_channel_resource(channel);
}
return 0;
}
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = -EINVAL;
if (mode != IOTimerChanMode_NotUsed) {
rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = allocate_channel_resource(channel, mode);
}
}
return rv;
}
static int timer_set_rate(unsigned timer, unsigned rate)
{
/* configure the timer to update at the desired rate */
rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
return 0;
}
static inline void io_timer_set_oneshot_mode(unsigned timer)
{
/* Ideally, we would want per channel One pulse mode in HW
* Alas OPE stops the Timer not the channel
* todo:We can do this in an ISR later
* But since we do not have that
* We try to get the longest rate we can.
* On 16 bit timers this is 8.1 Ms.
* On 32 but timers this is 536870.912
*/
rARR(timer) = 0xffffffff;
rPSC(timer) = (io_timers[timer].clock_freq / BOARD_ONESHOT_FREQ) - 1;
rEGR(timer) = GTIM_EGR_UG;
}
static inline void io_timer_set_PWM_mode(unsigned timer)
{
rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
}
void io_timer_trigger(void)
{
int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);
if (oneshots != 0) {
uint32_t action_cache[MAX_IO_TIMERS] = {0};
int actions = 0;
/* Pre-calculate the list of timers to Trigger */
for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
if (validate_timer_index(timer) == 0) {
int channels = get_timer_channels(timer);
if (oneshots & channels) {
action_cache[actions++] = io_timers[timer].base;
}
}
}
/* Now do them all with the shortest delay in between */
irqstate_t flags = px4_enter_critical_section();
for (actions = 0; action_cache[actions] != 0 && actions < MAX_IO_TIMERS; actions++) {
_REG32(action_cache[actions], STM32_GTIM_EGR_OFFSET) |= GTIM_EGR_UG;
}
px4_leave_critical_section(flags);
}
}
int io_timer_init_timer(unsigned timer)
{
/* Do this only once per timer */
int rv = is_timer_uninitalized(timer);
if (rv == 0) {
irqstate_t flags = px4_enter_critical_section();
set_timer_initalized(timer);
/* enable the timer clock before we try to talk to it */
modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit);
/* disable and configure the timer */
rCR1(timer) = 0;
rCR2(timer) = 0;
rSMCR(timer) = 0;
rDIER(timer) = 0;
rCCER(timer) = 0;
rCCMR1(timer) = 0;
rCCMR2(timer) = 0;
rCCR1(timer) = 0;
rCCR2(timer) = 0;
rCCR3(timer) = 0;
rCCR4(timer) = 0;
rCCER(timer) = 0;
rDCR(timer) = 0;
if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) {
/* master output enable = on */
rBDTR(timer) = ATIM_BDTR_MOE;
}
io_timer_set_PWM_mode(timer);
/*
* Note we do the Standard PWM Out init here
* default to updating at 50Hz
*/
timer_set_rate(timer, 50);
/*
* Note that the timer is left disabled with IRQ subs installed
* and active but DEIR bits are not set.
*/
irq_attach(io_timers[timer].vectorno, io_timers[timer].handler, NULL);
up_enable_irq(io_timers[timer].vectorno);
px4_leave_critical_section(flags);
}
return rv;
}
int io_timer_set_rate(unsigned timer, unsigned rate)
{
int rv = EBUSY;
/* Get the channel bits that belong to the timer */
uint32_t channels = get_timer_channels(timer);
/* Check that all channels are either in PWM or Oneshot */
if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
channel_allocations[IOTimerChanMode_OneShot] |
channel_allocations[IOTimerChanMode_NotUsed])) ==
channels) {
/* Change only a timer that is owned by pwm or one shot */
/* Request to use OneShot ?*/
if (rate == 0) {
/* Request to use OneShot
*
* We are here because ALL these channels were either PWM or Oneshot
* Now they need to be Oneshot
*/
/* Did the allocation change */
if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
io_timer_set_oneshot_mode(timer);
}
} else {
/* Request to use PWM
*
* We are here because ALL these channels were either PWM or Oneshot
* Now they need to be PWM
*/
if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
io_timer_set_PWM_mode(timer);
}
timer_set_rate(timer, rate);
}
rv = OK;
}
return rv;
}
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context)
{
uint32_t gpio = 0;
uint32_t clearbits = CCMR_C1_RESET;
uint32_t setbits = CCMR_C1_CAPTURE_INIT;
uint32_t ccer_setbits = CCER_C1_INIT;
uint32_t dier_setbits = GTIM_DIER_CC1IE;
/* figure out the GPIO config first */
switch (mode) {
case IOTimerChanMode_OneShot:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_Trigger:
ccer_setbits = 0;
dier_setbits = 0;
setbits = CCMR_C1_PWMOUT_INIT;
break;
case IOTimerChanMode_PWMIn:
setbits = CCMR_C1_PWMIN_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
#if defined(BOARD_HAS_CAPTURE)
case IOTimerChanMode_Capture:
setbits = CCMR_C1_CAPTURE_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
#endif
case IOTimerChanMode_NotUsed:
setbits = 0;
break;
default:
return -EINVAL;
}
int rv = allocate_channel(channel, mode);
/* Valid channel should now be reserved in new mode */
if (rv >= 0) {
/* Blindly try to initialize the timer - it will only do it once */
io_timer_init_timer(channels_timer(channel));
irqstate_t flags = px4_enter_critical_section();
/* Set up IO */
if (gpio) {
px4_arch_configgpio(gpio);
}
unsigned timer = channels_timer(channel);
/* configure the channel */
uint32_t shifts = timer_io_channels[channel].timer_channel - 1;
/* Map shifts timer channel 1-4 to 0-3 */
uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t));
uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t));
clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
uint16_t rvalue = REG(timer, ccmr_offset);
rvalue &= ~clearbits;
rvalue |= setbits;
REG(timer, ccmr_offset) = rvalue;
/*
* The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode
* As soon as CCxS (in SSMRx becomes different from 00, the channel is configured
* in input and the TIMx_CCR1 register becomes read-only.
* so the next line does nothing in capture mode and initializes an PWM out to
* 0
*/
REG(timer, ccr_offset) = 0;
/* on PWM Out ccer_setbits is 0 */
clearbits = (GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS);
setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS);
rvalue = rCCER(timer);
rvalue &= ~clearbits;
rvalue |= setbits;
rCCER(timer) = rvalue;
#if !defined(BOARD_HAS_CAPTURE)
UNUSED(dier_setbits);
#else
channel_handlers[channel].callback = channel_handler;
channel_handlers[channel].context = context;
rDIER(timer) |= dier_setbits << shifts;
#endif
px4_leave_critical_section(flags);
}
return rv;
}
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
{
struct action_cache_t {
uint32_t ccer_clearbits;
uint32_t ccer_setbits;
uint32_t dier_setbits;
uint32_t dier_clearbits;
uint32_t base;
uint32_t gpio[MAX_CHANNELS_PER_TIMER];
} action_cache[MAX_IO_TIMERS];
memset(action_cache, 0, sizeof(action_cache));
uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0;
uint32_t ccer_bit = state ? CCER_C1_INIT : 0;
switch (mode) {
case IOTimerChanMode_NotUsed:
case IOTimerChanMode_OneShot:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_Trigger:
dier_bit = 0;
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
break;
default:
return -EINVAL;
}
/* Was the request for all channels in this mode ?*/
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
/* Yes - we provide them */
masks = channel_allocations[mode];
} else {
/* No - caller provided mask */
/* Only allow the channels in that mode to be affected */
masks &= channel_allocations[mode];
}
/* Pre calculate all the changes */
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
if (masks & (1 << chan_index)) {
masks &= ~(1 << chan_index);
uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1;
uint32_t timer = channels_timer(chan_index);
action_cache[timer].base = io_timers[timer].base;
action_cache[timer].ccer_clearbits |= CCER_C1_INIT << (shifts * CCER_C1_NUM_BITS);
action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS);
action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts;
action_cache[timer].dier_setbits |= dier_bit << shifts;
if ((state &&
(mode == IOTimerChanMode_PWMOut ||
mode == IOTimerChanMode_OneShot ||
mode == IOTimerChanMode_Trigger))) {
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
}
}
}
irqstate_t flags = px4_enter_critical_section();
for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
if (action_cache[actions].base != 0) {
uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET);
rvalue &= ~action_cache[actions].ccer_clearbits;
rvalue |= action_cache[actions].ccer_setbits;
_REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue;
uint32_t after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E);
rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET);
rvalue &= ~action_cache[actions].dier_clearbits;
rvalue |= action_cache[actions].dier_setbits;
_REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue;
/* Any On ?*/
if (after != 0) {
/* force an update to preload all registers */
rEGR(actions) = GTIM_EGR_UG;
for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) {
if (action_cache[actions].gpio[chan]) {
px4_arch_configgpio(action_cache[actions].gpio[chan]);
action_cache[actions].gpio[chan] = 0;
}
}
/* arm requires the timer be enabled */
rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
} else {
rCR1(actions) = 0;
}
}
}
px4_leave_critical_section(flags);
return 0;
}
int io_timer_set_ccr(unsigned channel, uint16_t value)
{
int rv = io_timer_validate_channel_index(channel);
int mode = io_timer_get_channel_mode(channel);
if (rv == 0) {
if ((mode != IOTimerChanMode_PWMOut) &&
(mode != IOTimerChanMode_OneShot) &&
(mode != IOTimerChanMode_Trigger)) {
rv = -EIO;
} else {
/* configure the channel */
REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value;
}
}
return rv;
}
uint16_t io_channel_get_ccr(unsigned channel)
{
uint16_t value = 0;
if (io_timer_validate_channel_index(channel) == 0) {
int mode = io_timer_get_channel_mode(channel);
if ((mode == IOTimerChanMode_PWMOut) ||
(mode == IOTimerChanMode_OneShot) ||
(mode == IOTimerChanMode_Trigger)) {
value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset);
}
}
return value;
}
uint32_t io_timer_get_group(unsigned timer)
{
return get_timer_channels(timer);
}
src/drivers/stm32/drv_io_timer.h
-136
View File
@@ -1,136 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_io_timer.h
*
* stm32-specific PWM output data.
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <drivers/drv_hrt.h>
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 5
#define MAX_TIMER_IO_CHANNELS 8
#define MAX_LED_TIMERS 2
#define MAX_TIMER_LED_CHANNELS 6
#define IO_TIMER_ALL_MODES_CHANNELS 0
typedef enum io_timer_channel_mode_t {
IOTimerChanMode_NotUsed = 0,
IOTimerChanMode_PWMOut = 1,
IOTimerChanMode_PWMIn = 2,
IOTimerChanMode_Capture = 3,
IOTimerChanMode_OneShot = 4,
IOTimerChanMode_Trigger = 5,
IOTimerChanModeSize
} io_timer_channel_mode_t;
typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
/* array of timers dedicated to PWM in and out and capture use
*** Note that the clock_freq is set to the source in the clock tree that
*** feeds this specific timer. This can differs by Timer!
*** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz
*** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz
*** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz
*** the resulting PSC will be one and the timer will count at it's clock frequency.
*/
typedef struct io_timers_t {
uint32_t base;
uint32_t clock_register;
uint32_t clock_bit;
uint32_t clock_freq;
uint32_t vectorno;
uint32_t first_channel_index;
uint32_t last_channel_index;
xcpt_t handler;
} io_timers_t;
/* array of channels in logical order */
typedef struct timer_io_channels_t {
uint32_t gpio_out;
uint32_t gpio_in;
uint8_t timer_index;
uint8_t timer_channel;
uint16_t masks;
uint8_t ccr_offset;
} timer_io_channels_t;
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time, uint16_t isrs_rcnt);
/* supplied by board-specific code */
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
__EXPORT int io_timer_handler0(int irq, void *context, void *arg);
__EXPORT int io_timer_handler1(int irq, void *context, void *arg);
__EXPORT int io_timer_handler2(int irq, void *context, void *arg);
__EXPORT int io_timer_handler3(int irq, void *context, void *arg);
__EXPORT int io_timer_handler4(int irq, void *context, void *arg);
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context);
__EXPORT int io_timer_init_timer(unsigned timer);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
io_timer_channel_allocation_t masks);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
__EXPORT uint32_t io_timer_get_group(unsigned timer);
__EXPORT int io_timer_validate_channel_index(unsigned channel);
__EXPORT int io_timer_is_channel_free(unsigned channel);
__EXPORT int io_timer_free_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__EXPORT extern void io_timer_trigger(void);
__END_DECLS
src/drivers/stm32/drv_pwm_servo.c
-170
View File
@@ -1,170 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_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 <px4_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_io_timer.h"
#include "drv_pwm_servo.h"
#include <stm32_tim.h>
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
return io_timer_set_ccr(channel, value);
}
servo_position_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}
int up_pwm_servo_init(uint32_t channel_mask)
{
/* Init channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) |
io_timer_get_mode_channels(IOTimerChanMode_OneShot);
/* First free the current set of PWMs */
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (current & (1 << channel)) {
io_timer_free_channel(channel);
current &= ~(1 << channel);
}
}
/* Now allocate the new set */
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
/* First free any that were not PWM mode before */
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
/* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
channel_mask &= ~(1 << channel);
}
}
return OK;
}
void up_pwm_servo_deinit(void)
{
/* disable the timers */
up_pwm_servo_arm(false);
}
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) {
return ERROR;
}
/* Allow a rate of 0 to enter oneshot mode */
if (rate != 0) {
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1) {
return -ERANGE;
}
if (rate > 10000) {
return -ERANGE;
}
}
return io_timer_set_rate(group, rate);
}
void up_pwm_update(void)
{
io_timer_trigger();
}
int up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
up_pwm_servo_set_rate_group_update(i, rate);
}
return 0;
}
uint32_t up_pwm_servo_get_rate_group(unsigned group)
{
/* only return the set of channels in the group which we own */
return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) |
io_timer_get_mode_channels(IOTimerChanMode_OneShot)) &
io_timer_get_group(group);
}
void
up_pwm_servo_arm(bool armed)
{
io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
}
src/drivers/stm32/drv_pwm_servo.h
-43
View File
@@ -1,43 +0,0 @@
/****************************************************************************
*
* 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>
src/drivers/stm32/drv_pwm_trigger.c
-110
View File
@@ -1,110 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_pwm_trigger.c
*
*/
#include <px4_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_trigger.h>
#include "drv_io_timer.h"
#include "drv_pwm_trigger.h"
#include <stm32_tim.h>
int up_pwm_trigger_set(unsigned channel, uint16_t value)
{
return io_timer_set_ccr(channel, value);
}
int up_pwm_trigger_init(uint32_t channel_mask)
{
/* Init channels */
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
// First free any that were not trigger mode before
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
channel_mask &= ~(1 << channel);
}
}
/* Enable the timers */
up_pwm_trigger_arm(true);
return OK;
}
void up_pwm_trigger_deinit()
{
/* Disable the timers */
up_pwm_trigger_arm(false);
/* Deinit channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_Trigger);
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (current & (1 << channel)) {
io_timer_channel_init(channel, IOTimerChanMode_NotUsed, NULL, NULL);
current &= ~(1 << channel);
}
}
}
void
up_pwm_trigger_arm(bool armed)
{
io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS);
}
src/drivers/stm32/drv_pwm_trigger.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_pwm_trigger.h
*
* stm32-specific PWM output data.
*/
#pragma once
#include <drivers/drv_pwm_trigger.h>
src/platforms/px4_micro_hal.h
+1 -1
View File
@@ -37,5 +37,5 @@
*/
// include arch-specific header
#include <px4_arch_micro_hal.h>
#include <px4_arch/micro_hal.h>