diff options
author | px4dev <px4@purgatory.org> | 2012-10-23 21:43:41 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-10-23 23:51:13 -0700 |
commit | 3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a (patch) | |
tree | a477c4ef7c88e7a6a2402207070af3160080fcf6 /nuttx/configs | |
parent | c3fe915b44a1b32f05b182b3079c722b82b20fb9 (diff) | |
download | px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.tar.gz px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.tar.bz2 px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.zip |
Tease the PWM driver out and fix some build issues after cleaning up behind the cpuload pieces.
Diffstat (limited to 'nuttx/configs')
-rw-r--r-- | nuttx/configs/px4fmu/include/up_cpuload.h | 62 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/Makefile | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_cpuload.c | 183 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_nsh.c | 280 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_pwm_servo.c | 346 |
6 files changed, 2 insertions, 872 deletions
diff --git a/nuttx/configs/px4fmu/include/up_cpuload.h b/nuttx/configs/px4fmu/include/up_cpuload.h deleted file mode 100644 index b61c5c550..000000000 --- a/nuttx/configs/px4fmu/include/up_cpuload.h +++ /dev/null @@ -1,62 +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. - * - ****************************************************************************/ - -#ifndef UP_CPULOAD_H_ -#define UP_CPULOAD_H_ - -#ifdef CONFIG_SCHED_INSTRUMENTATION - -#include <nuttx/sched.h> - -struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task - FAR struct _TCB *tcb; ///< - bool valid; ///< Task is currently active / valid -}; - -struct system_load_s { - uint64_t start_time; ///< Global start time of measurements - struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; - uint8_t initialized; - int total_count; - int running_count; - int sleeping_count; -}; - -void cpuload_initialize_once(void); - -#endif - -#endif diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index b587b3386..cfd86aac1 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -95,6 +95,7 @@ CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io +CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += px4/fmu diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 0768a9d60..f6140fb93 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -43,7 +43,7 @@ AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_boot.c up_leds.c up_spi.c \ drv_gpio.c \ drv_led.c drv_eeprom.c \ - up_pwm_servo.c up_usbdev.c + up_usbdev.c ifeq ($(CONFIG_ADC),y) CSRCS += up_adc.c diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/nuttx/configs/px4fmu/src/up_cpuload.c deleted file mode 100644 index bf867ae8b..000000000 --- a/nuttx/configs/px4fmu/src/up_cpuload.c +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/src/up_leds.c - * arch/arm/src/board/up_leds.c - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <gnutt@nuttx.org> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <nuttx/config.h> - -#include <stdint.h> -#include <stdbool.h> -#include <debug.h> - -#include <sys/time.h> -#include <sched.h> - -#include <arch/board/board.h> -#include <arch/board/up_hrt.h> -#include <arch/board/up_cpuload.h> - -#include "chip.h" -#include "up_arch.h" -#include "up_internal.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: - ****************************************************************************/ - -#ifdef CONFIG_SCHED_INSTRUMENTATION - -struct system_load_s system_load; - -extern FAR _TCB *sched_gettcb(pid_t pid); - -void cpuload_initialize_once(void); - -void cpuload_initialize_once() -{ -// if (!system_load.initialized) -// { - system_load.start_time = hrt_absolute_time(); - int i; - for (i = 0; i < CONFIG_MAX_TASKS; i++) - { - system_load.tasks[i].valid = false; - } - system_load.total_count = 0; - - uint64_t now = hrt_absolute_time(); - - /* initialize idle thread statically */ - system_load.tasks[0].start_time = now; - system_load.tasks[0].total_runtime = 0; - system_load.tasks[0].curr_start_time = 0; - system_load.tasks[0].tcb = sched_gettcb(0); - system_load.tasks[0].valid = true; - system_load.total_count++; - - /* initialize init thread statically */ - system_load.tasks[1].start_time = now; - system_load.tasks[1].total_runtime = 0; - system_load.tasks[1].curr_start_time = 0; - system_load.tasks[1].tcb = sched_gettcb(1); - system_load.tasks[1].valid = true; - /* count init thread */ - system_load.total_count++; - // } -} - -void sched_note_start(FAR _TCB *tcb ) -{ - /* search first free slot */ - int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (!system_load.tasks[i].valid) - { - /* slot is available */ - system_load.tasks[i].start_time = hrt_absolute_time(); - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = tcb; - system_load.tasks[i].valid = true; - system_load.total_count++; - break; - } - } -} - -void sched_note_stop(FAR _TCB *tcb ) -{ - int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (system_load.tasks[i].tcb->pid == tcb->pid) - { - /* mark slot as fee */ - system_load.tasks[i].valid = false; - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = NULL; - system_load.total_count--; - break; - } - } -} - -void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) -{ - uint64_t new_time = hrt_absolute_time(); - - /* Kind of inefficient: find both tasks and update times */ - uint8_t both_found = 0; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) - { - /* Task ending its current scheduling run */ - if (system_load.tasks[i].tcb->pid == pFromTcb->pid) - { - //if (system_load.tasks[i].curr_start_time != 0) - { - system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; - } - both_found++; - } - else if (system_load.tasks[i].tcb->pid == pToTcb->pid) - { - system_load.tasks[i].curr_start_time = new_time; - both_found++; - } - - /* Do only iterate as long as needed */ - if (both_found == 2) - { - break; - } - } -} - -#endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c deleted file mode 100644 index 763207d3a..000000000 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ /dev/null @@ -1,280 +0,0 @@ -/**************************************************************************** - * config/px4fmu/src/up_nsh.c - * arch/arm/src/board/up_nsh.c - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <spudmonkey@racsa.co.cr> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdio.h> -#include <debug.h> -#include <errno.h> - -#include <nuttx/arch.h> -#include <nuttx/spi.h> -#include <nuttx/i2c.h> -#include <nuttx/mmcsd.h> -#include <nuttx/analog/adc.h> -#include <nuttx/arch.h> - -#include "stm32_internal.h" -#include "px4fmu-internal.h" -#include "stm32_uart.h" - -#include <arch/board/up_hrt.h> -#include <arch/board/up_cpuload.h> -#include <arch/board/up_adc.h> -#include <arch/board/board.h> -#include <arch/board/drv_led.h> -#include <arch/board/drv_eeprom.h> - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lib_lowprintf(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lib_lowprintf -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi3; -static struct i2c_dev_s *i2c1; -static struct i2c_dev_s *i2c2; -static struct i2c_dev_s *i2c3; - -#include <math.h> - -#ifdef __cplusplus -int matherr(struct __exception *e) { - return 1; -} -#else -int matherr(struct exception *e) { - return 1; -} -#endif - -int nsh_archinitialize(void) -{ - int result; - - /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ - - /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ - - /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER - hrt_init(); -#endif - - /* configure CPU load estimation */ - #ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); - #endif - - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); - - up_ledoff(LED_BLUE); - up_ledoff(LED_AMBER); - - up_ledon(LED_BLUE); - - /* Configure user-space led driver */ - px4fmu_led_init(); - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - if (!spi1) - { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); - SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\r\n"); - - /* initialize I2C2 bus */ - - i2c2 = up_i2cinitialize(2); - if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C2 speed */ - I2C_SETFREQUENCY(i2c2, 400000); - - - i2c3 = up_i2cinitialize(3); - if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C3 speed */ - I2C_SETFREQUENCY(i2c3, 400000); - - /* try to attach, don't fail if device is not responding */ - (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, - FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); - -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ - - message("[boot] Initializing SPI port 3\n"); - spi3 = up_spiinitialize(3); - if (!spi3) - { - message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully initialized SPI port 3\n"); - - /* Now bind the SPI interface to the MMCSD driver */ - result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); - if (result != OK) - { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ - - /* initialize I2C1 bus */ - - i2c1 = up_i2cinitialize(1); - if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C1 speed */ - I2C_SETFREQUENCY(i2c1, 400000); - - /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ - - /* Get board information if available */ - - /* Initialize the user GPIOs */ - px4fmu_gpio_init(); - -#ifdef CONFIG_ADC - int adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Try again */ - adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Give up */ - message("[boot] FAILED adc_devinit: %d\n", adc_state); - return -ENODEV; - } - } -#endif - - return OK; -} diff --git a/nuttx/configs/px4fmu/src/up_pwm_servo.c b/nuttx/configs/px4fmu/src/up_pwm_servo.c deleted file mode 100644 index d278da55c..000000000 --- a/nuttx/configs/px4fmu/src/up_pwm_servo.c +++ /dev/null @@ -1,346 +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. - * - ****************************************************************************/ - -/* - * 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 <arch/board/up_pwm_servo.h> - -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" - -/* 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; - } -} |