mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 01:04:19 +08:00
Tease the PWM driver out and fix some build issues after cleaning up behind the cpuload pieces.
This commit is contained in:
px4dev
@@ -63,7 +63,6 @@
|
||||
#include "px4fmu_internal.h"
|
||||
#include "stm32_uart.h"
|
||||
|
||||
#include <arch/board/up_cpuload.h>
|
||||
#include <arch/board/up_adc.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <arch/board/drv_led.h>
|
||||
@@ -71,6 +70,8 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
@@ -41,14 +41,15 @@
|
||||
* channel.
|
||||
*/
|
||||
|
||||
#ifndef _DRV_PWM_OUTPUT_H
|
||||
#define _DRV_PWM_OUTPUT_H
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* Path for the default PWM output device.
|
||||
*
|
||||
@@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm);
|
||||
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
|
||||
|
||||
|
||||
#endif /* _DRV_PWM_OUTPUT_H */
|
||||
/*
|
||||
* Low-level PWM output interface.
|
||||
*
|
||||
* This is the low-level API to the platform-specific PWM driver.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Intialise the PWM servo outputs using the specified configuration.
|
||||
*
|
||||
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
|
||||
* This allows some of the channels to remain configured
|
||||
* as GPIOs or as another function.
|
||||
* @return OK on success.
|
||||
*/
|
||||
__EXPORT extern int up_pwm_servo_init(uint32_t channel_mask);
|
||||
|
||||
/**
|
||||
* De-initialise the PWM servo outputs.
|
||||
*/
|
||||
__EXPORT extern void up_pwm_servo_deinit(void);
|
||||
|
||||
/**
|
||||
* Arm or disarm servo outputs.
|
||||
*
|
||||
* When disarmed, servos output no pulse.
|
||||
*
|
||||
* @bug This function should, but does not, guarantee that any pulse
|
||||
* currently in progress is cleanly completed.
|
||||
*
|
||||
* @param armed If true, outputs are armed; if false they
|
||||
* are disarmed.
|
||||
*/
|
||||
__EXPORT extern void up_pwm_servo_arm(bool armed);
|
||||
|
||||
/**
|
||||
* Set the servo update rate
|
||||
*
|
||||
* @param rate The update rate in Hz to set.
|
||||
* @return OK on success, -ERANGE if an unsupported update rate is set.
|
||||
*/
|
||||
__EXPORT extern int up_pwm_servo_set_rate(unsigned rate);
|
||||
|
||||
/**
|
||||
* Set the current output value for a channel.
|
||||
*
|
||||
* @param channel The channel to set.
|
||||
* @param value The output pulse width in microseconds.
|
||||
*/
|
||||
__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value);
|
||||
|
||||
/**
|
||||
* Get the current output value for a channel.
|
||||
*
|
||||
* @param channel The channel to read.
|
||||
* @return The output pulse width in microseconds, or zero if
|
||||
* outputs are not armed or not configured.
|
||||
*/
|
||||
__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# STM32 driver support code
|
||||
#
|
||||
# Modules in this directory are compiled for all STM32 targets.
|
||||
#
|
||||
|
||||
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
@@ -0,0 +1,348 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_pwm_servo.c
|
||||
*
|
||||
* Servo driver supporting PWM servos connected to STM32 timer blocks.
|
||||
*
|
||||
* Works with any of the 'generic' or 'advanced' STM32 timers that
|
||||
* have output pins, does not require an interrupt.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/irq.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <time.h>
|
||||
#include <queue.h>
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "up_internal.h"
|
||||
#include "up_arch.h"
|
||||
|
||||
#include "stm32_internal.h"
|
||||
#include "stm32_gpio.h"
|
||||
#include "stm32_tim.h"
|
||||
|
||||
/* configuration limits */
|
||||
#define PWM_SERVO_MAX_TIMERS 2
|
||||
#define PWM_SERVO_MAX_CHANNELS 8
|
||||
|
||||
/* default rate (in Hz) of PWM updates */
|
||||
static uint32_t pwm_update_rate = 50;
|
||||
|
||||
/*
|
||||
* Servo configuration for all of the pins that can be used as
|
||||
* PWM outputs on FMU.
|
||||
*/
|
||||
|
||||
/* array of timers dedicated to PWM servo use */
|
||||
static const struct pwm_servo_timer {
|
||||
uint32_t base;
|
||||
uint32_t clock_register;
|
||||
uint32_t clock_bit;
|
||||
uint32_t clock_freq;
|
||||
} pwm_timers[] = {
|
||||
{
|
||||
.base = STM32_TIM2_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM2EN,
|
||||
.clock_freq = STM32_APB1_TIM2_CLKIN
|
||||
}
|
||||
};
|
||||
|
||||
/* array of channels in logical order */
|
||||
static const struct pwm_servo_channel {
|
||||
uint32_t gpio;
|
||||
uint8_t timer_index;
|
||||
uint8_t timer_channel;
|
||||
servo_position_t default_value;
|
||||
} pwm_channels[] = {
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH1OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 1,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH2OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH3OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH4OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 4,
|
||||
.default_value = 1000,
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
|
||||
|
||||
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
|
||||
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
|
||||
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
|
||||
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
|
||||
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
|
||||
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
|
||||
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
|
||||
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
|
||||
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
|
||||
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
|
||||
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
|
||||
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
|
||||
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
|
||||
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
|
||||
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
|
||||
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
|
||||
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
|
||||
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
|
||||
|
||||
static void
|
||||
pwm_timer_init(unsigned timer)
|
||||
{
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
|
||||
|
||||
/* disable and configure the timer */
|
||||
rCR1(timer) = 0;
|
||||
rCR2(timer) = 0;
|
||||
rSMCR(timer) = 0;
|
||||
rDIER(timer) = 0;
|
||||
rCCER(timer) = 0;
|
||||
rCCMR1(timer) = 0;
|
||||
rCCMR2(timer) = 0;
|
||||
rCCER(timer) = 0;
|
||||
rDCR(timer) = 0;
|
||||
|
||||
/* configure the timer to free-run at 1MHz */
|
||||
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
|
||||
|
||||
/* and update at the desired rate */
|
||||
rARR(timer) = (1000000 / pwm_update_rate) - 1;
|
||||
|
||||
/* generate an update event; reloads the counter and all registers */
|
||||
rEGR(timer) = GTIM_EGR_UG;
|
||||
|
||||
/* note that the timer is left disabled - arming is performed separately */
|
||||
}
|
||||
|
||||
static void
|
||||
pwm_timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
/* configure the timer to update at the desired rate */
|
||||
rARR(timer) = 1000000 / rate;
|
||||
|
||||
/* generate an update event; reloads the counter and all registers */
|
||||
rEGR(timer) = GTIM_EGR_UG;
|
||||
}
|
||||
|
||||
static void
|
||||
pwm_channel_init(unsigned channel)
|
||||
{
|
||||
unsigned timer = pwm_channels[channel].timer_index;
|
||||
|
||||
/* configure the GPIO first */
|
||||
stm32_configgpio(pwm_channels[channel].gpio);
|
||||
|
||||
/* configure the channel */
|
||||
switch (pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
rCCMR1(timer) |= (6 << 4);
|
||||
rCCR1(timer) = pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= (1 << 0);
|
||||
break;
|
||||
case 2:
|
||||
rCCMR1(timer) |= (6 << 12);
|
||||
rCCR2(timer) = pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= (1 << 4);
|
||||
break;
|
||||
case 3:
|
||||
rCCMR2(timer) |= (6 << 4);
|
||||
rCCR3(timer) = pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= (1 << 8);
|
||||
break;
|
||||
case 4:
|
||||
rCCMR2(timer) |= (6 << 12);
|
||||
rCCR4(timer) = pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= (1 << 12);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_set(unsigned channel, servo_position_t value)
|
||||
{
|
||||
if (channel >= PWM_SERVO_MAX_CHANNELS) {
|
||||
lldbg("pwm_channel_set: bogus channel %u\n", channel);
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned timer = pwm_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((pwm_timers[timer].base == 0) ||
|
||||
(pwm_channels[channel].gpio == 0))
|
||||
return -1;
|
||||
|
||||
/* configure the channel */
|
||||
if (value > 0)
|
||||
value--;
|
||||
switch (pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
rCCR1(timer) = value;
|
||||
break;
|
||||
case 2:
|
||||
rCCR2(timer) = value;
|
||||
break;
|
||||
case 3:
|
||||
rCCR3(timer) = value;
|
||||
break;
|
||||
case 4:
|
||||
rCCR4(timer) = value;
|
||||
break;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
servo_position_t
|
||||
up_pwm_servo_get(unsigned channel)
|
||||
{
|
||||
if (channel >= PWM_SERVO_MAX_CHANNELS) {
|
||||
lldbg("pwm_channel_get: bogus channel %u\n", channel);
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned timer = pwm_channels[channel].timer_index;
|
||||
servo_position_t value = 0;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((pwm_timers[timer].base == 0) ||
|
||||
(pwm_channels[channel].gpio == 0))
|
||||
return 0;
|
||||
|
||||
/* configure the channel */
|
||||
switch (pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
value = rCCR1(timer);
|
||||
break;
|
||||
case 2:
|
||||
value = rCCR2(timer);
|
||||
break;
|
||||
case 3:
|
||||
value = rCCR3(timer);
|
||||
break;
|
||||
case 4:
|
||||
value = rCCR4(timer);
|
||||
break;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_init(uint32_t channel_mask)
|
||||
{
|
||||
/* do basic timer initialisation first */
|
||||
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
|
||||
if (pwm_timers[i].base != 0)
|
||||
pwm_timer_init(i);
|
||||
}
|
||||
|
||||
/* now init channels */
|
||||
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
|
||||
/* don't do init for disabled channels; this leaves the pin configs alone */
|
||||
if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0))
|
||||
pwm_channel_init(i);
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_deinit(void)
|
||||
{
|
||||
/* disable the timers */
|
||||
up_pwm_servo_arm(false);
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_set_rate(unsigned rate)
|
||||
{
|
||||
if ((rate < 50) || (rate > 400))
|
||||
return -ERANGE;
|
||||
|
||||
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
|
||||
if (pwm_timers[i].base != 0)
|
||||
pwm_timer_set_rate(i, rate);
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_arm(bool armed)
|
||||
{
|
||||
/*
|
||||
* XXX this is inelgant and in particular will either jam outputs at whatever level
|
||||
* they happen to be at at the time the timers stop or generate runts.
|
||||
* The right thing is almost certainly to kill auto-reload on the timers so that
|
||||
* they just stop at the end of their count for disable, and to reset/restart them
|
||||
* for enable.
|
||||
*/
|
||||
|
||||
/* iterate timers and arm/disarm appropriately */
|
||||
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
|
||||
if (pwm_timers[i].base != 0)
|
||||
rCR1(i) = armed ? GTIM_CR1_CEN : 0;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user