mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-26 02:20:36 +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
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 */
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user