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

Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-26 21:21:07 +02:00
parent b9d03c7c27 241a0d8653
commit f321e7f419
133 changed files with 2423 additions and 4388 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nuttx/configs/px4fmu/include/up_adc.h
-60
View File
@@ -1,60 +0,0 @@
/************************************************************************************
* configs/stm3240g-eval/src/up_adc.c
* arch/arm/src/board/up_adc.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>
#ifdef CONFIG_ADC
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: adc_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/adc.
*
************************************************************************************/
int adc_devinit(void);
#endif /* CONFIG_ADC */
nuttx/configs/px4fmu/include/up_cpuload.h
-62
View File
@@ -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
nuttx/configs/px4fmu/include/up_hrt.h
-130
View File
@@ -1,130 +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.
*
****************************************************************************/
/*
* High-resolution timer callouts and timekeeping.
*/
#ifndef UP_HRT_H_
#define UP_HRT_H_
#include <sys/types.h>
#include <stdbool.h>
#include <time.h>
#include <queue.h>
/*
* Absolute time, in microsecond units.
*
* Absolute time is measured from some arbitrary epoch shortly after
* system startup. It should never wrap or go backwards.
*/
typedef uint64_t hrt_abstime;
/*
* Callout function type.
*
* Note that callouts run in the timer interrupt context, so
* they are serialised with respect to each other, and must not
* block.
*/
typedef void (* hrt_callout)(void *arg);
/*
* Callout record.
*/
typedef struct hrt_call {
struct sq_entry_s link;
hrt_abstime deadline;
hrt_abstime period;
hrt_callout callout;
void *arg;
} *hrt_call_t;
/*
* Get absolute time.
*/
extern hrt_abstime hrt_absolute_time(void);
/*
* Convert a timespec to absolute time.
*/
extern hrt_abstime ts_to_abstime(struct timespec *ts);
/*
* Convert absolute time to a timespec.
*/
extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
* with hrt_called().
*/
extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
/*
* Call callout(arg) at absolute time calltime.
*/
extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
/*
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
* jitter but should not drift.
*/
extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
/*
* If this returns true, the entry has been invoked and removed from the callout list,
* or it has never been entered.
*
* Always returns false for repeating callouts.
*/
extern bool hrt_called(struct hrt_call *entry);
/*
* Remove the entry from the callout list.
*/
extern void hrt_cancel(struct hrt_call *entry);
/*
* Initialise the HRT.
*/
extern void hrt_init(void);
#endif /* UP_HRT_H_ */
nuttx/configs/px4fmu/include/up_pwm_servo.h
-117
View File
@@ -1,117 +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.
*
****************************************************************************/
/*
* Low-level PWM servo control.
*
* The pwm_servo module supports servos connected to STM32 timer
* blocks.
*
* On PX4FMU, the outputs are:
*
* 0 : USART2/multi CTS
* 1 : USART2/multi RTS
* 2 : USART2/multi TX
* 3 : USART2/multi RX
* 4 : CAN2 TX
* 5 : CAN2 RX
*/
#ifndef UP_PWM_SERVO_H
#define UP_PWM_SERVO_H
typedef uint16_t servo_position_t;
#if defined(__cplusplus)
extern "C" {
#endif
/**
* 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.
*/
extern int up_pwm_servo_init(uint32_t channel_mask);
/**
* De-initialise the PWM servo outputs.
*/
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.
*/
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.
*/
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.
*/
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.
*/
extern servo_position_t up_pwm_servo_get(unsigned channel);
#if defined(__cplusplus)
}
#endif
#endif /* UP_PWM_SERVO_H */
nuttx/configs/px4fmu/nsh/appconfig
+3 -3
View File
@@ -76,27 +76,27 @@ CONFIGURED_APPS += sensors
CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
CONFIGURED_APPS += px4/attitude_estimator_bm
CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += fixedwing_att_control
CONFIGURED_APPS += fixedwing_pos_control
CONFIGURED_APPS += position_estimator
CONFIGURED_APPS += attitude_estimator_ekf
CONFIGURED_APPS += px4/ground_estimator
# Hacking tools
#CONFIGURED_APPS += system/i2c
#CONFIGURED_APPS += tools/i2c_dev
# Communication and Drivers
CONFIGURED_APPS += drivers/boards/px4fmu
CONFIGURED_APPS += drivers/device
CONFIGURED_APPS += drivers/ms5611
CONFIGURED_APPS += drivers/hmc5883
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/px4io/driver
CONFIGURED_APPS += px4/fmu
# Testing stuff
nuttx/configs/px4fmu/src/Makefile
+2 -16
View File
@@ -40,23 +40,9 @@ CFLAGS += -I$(TOPDIR)/sched
ASRCS =
AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \
CSRCS = up_leds.c \
drv_gpio.c \
drv_led.c drv_eeprom.c \
up_pwm_servo.c up_usbdev.c \
up_cpuload.c
ifeq ($(CONFIG_NSH_ARCHINIT),y)
CSRCS += up_nsh.c
endif
ifeq ($(CONFIG_ADC),y)
CSRCS += up_adc.c
endif
ifeq ($(CONFIG_CAN),y)
CSRCS += up_can.c
endif
drv_led.c drv_eeprom.c
COBJS = $(CSRCS:.c=$(OBJEXT))
nuttx/configs/px4fmu/src/up_adc.c
-173
View File
@@ -1,173 +0,0 @@
/************************************************************************************
* configs/stm3240g-eval/src/up_adc.c
* arch/arm/src/board/up_adc.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 <errno.h>
#include <debug.h>
#include <stdio.h>
#include <nuttx/analog/adc.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
//#include "stm32_pwm.h"
#include "stm32_adc.h"
#include "px4fmu-internal.h"
#ifdef CONFIG_ADC
/************************************************************************************
* Definitions
************************************************************************************/
/* Configuration ************************************************************/
/* Up to 3 ADC interfaces are supported */
#if STM32_NADC < 3
# undef CONFIG_STM32_ADC3
#endif
#if STM32_NADC < 2
# undef CONFIG_STM32_ADC2
#endif
#if STM32_NADC < 1
# undef CONFIG_STM32_ADC3
#endif
#if defined(CONFIG_STM32_ADC1) || defined(CONFIG_STM32_ADC2) || defined(CONFIG_STM32_ADC3)
#ifndef CONFIG_STM32_ADC3
# warning "Channel information only available for ADC3"
#endif
#define ADC3_NCHANNELS 4
/************************************************************************************
* Private Data
************************************************************************************/
/* The PX4FMU board has four ADC channels: ADC323 IN10-13
*/
/* Identifying number of each ADC channel: Variable Resistor. */
#ifdef CONFIG_STM32_ADC3
static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards
/* Configurations of pins used byte each ADC channels */
static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: adc_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/adc.
*
************************************************************************************/
int adc_devinit(void)
{
#ifdef CONFIG_STM32_ADC3
static bool initialized = false;
struct adc_dev_s *adc[ADC3_NCHANNELS];
int ret;
int i;
/* Check if we have already initialized */
if (!initialized)
{
char name[11];
for (i = 0; i < ADC3_NCHANNELS; i++)
{
stm32_configgpio(g_pinlist[i]);
}
for (i = 0; i < 1; i++)
{
/* Configure the pins as analog inputs for the selected channels */
//stm32_configgpio(g_pinlist[i]);
/* Call stm32_adcinitialize() to get an instance of the ADC interface */
//multiple channels only supported with dma!
adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
if (adc == NULL)
{
adbg("ERROR: Failed to get ADC interface\n");
return -ENODEV;
}
/* Register the ADC driver at "/dev/adc0" */
sprintf(name, "/dev/adc%d", i);
ret = adc_register(name, adc[i]);
if (ret < 0)
{
adbg("adc_register failed for adc %s: %d\n", name, ret);
return ret;
}
}
/* Now we are initialized */
initialized = true;
}
return OK;
#else
return -ENOSYS;
#endif
}
#endif /* CONFIG_STM32_ADC || CONFIG_STM32_ADC2 || CONFIG_STM32_ADC3 */
#endif /* CONFIG_ADC */
nuttx/configs/px4fmu/src/up_boot.c
-76
View File
@@ -1,76 +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.
*
****************************************************************************/
/************************************************************************************
* Included Files
************************************************************************************/
#include <nuttx/config.h>
#include <debug.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "px4fmu-internal.h"
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
void stm32_boardinitialize(void)
{
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
up_ledinit();
}
nuttx/configs/px4fmu/src/up_can.c
-142
View File
@@ -1,142 +0,0 @@
/************************************************************************************
* configs/px4fmu/src/up_can.c
* arch/arm/src/board/up_can.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 <errno.h>
#include <debug.h>
#include <nuttx/can.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
#include "stm32.h"
#include "stm32_can.h"
#include "px4fmu-internal.h"
#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2))
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
/* Configuration ********************************************************************/
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
# undef CONFIG_STM32_CAN2
#endif
#ifdef CONFIG_STM32_CAN1
# define CAN_PORT 1
#else
# define CAN_PORT 2
#endif
/* Debug ***************************************************************************/
/* Non-standard debug that may be enabled just for testing CAN */
#ifdef CONFIG_DEBUG_CAN
# define candbg dbg
# define canvdbg vdbg
# define canlldbg lldbg
# define canllvdbg llvdbg
#else
# define candbg(x...)
# define canvdbg(x...)
# define canlldbg(x...)
# define canllvdbg(x...)
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: can_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/can.
*
************************************************************************************/
int can_devinit(void)
{
static bool initialized = false;
struct can_dev_s *can;
int ret;
/* Check if we have already initialized */
if (!initialized)
{
/* Call stm32_caninitialize() to get an instance of the CAN interface */
can = stm32_caninitialize(CAN_PORT);
if (can == NULL)
{
candbg("ERROR: Failed to get CAN interface\n");
return -ENODEV;
}
/* Register the CAN driver at "/dev/can0" */
ret = can_register("/dev/can0", can);
if (ret < 0)
{
candbg("ERROR: can_register failed: %d\n", ret);
return ret;
}
/* Now we are initialized */
initialized = true;
}
return OK;
}
#endif /* CONFIG_STM32_CAN || CONFIG_STM32_CAN2 || CONFIG_STM32_CAN3 */
nuttx/configs/px4fmu/src/up_cpuload.c
-183
View File
@@ -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 */
nuttx/configs/px4fmu/src/up_hrt.c
-843
View File
@@ -1,843 +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 High-resolution timer callouts and timekeeping.
*
* This can use any general or advanced STM32 timer.
*
* Note that really, this could use systick too, but that's
* monopolised by NuttX and stealing it would just be awkward.
*
* We don't use the NuttX STM32 driver per se; rather, we
* claim the timer and then drive it directly.
*/
#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 <arch/board/board.h>
#include <arch/board/up_hrt.h>
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#include "stm32_internal.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#ifdef CONFIG_HRT_TIMER
/* HRT configuration */
#if HRT_TIMER == 1
# define HRT_TIMER_BASE STM32_TIM1_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC
# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN
# if CONFIG_STM32_TIM1
# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
# endif
#elif HRT_TIMER == 2
# define HRT_TIMER_BASE STM32_TIM2_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM2
# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN
# if CONFIG_STM32_TIM2
# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
# endif
#elif HRT_TIMER == 3
# define HRT_TIMER_BASE STM32_TIM3_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM3
# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN
# if CONFIG_STM32_TIM3
# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
# endif
#elif HRT_TIMER == 4
# define HRT_TIMER_BASE STM32_TIM4_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM4
# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN
# if CONFIG_STM32_TIM4
# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
# endif
#elif HRT_TIMER == 5
# define HRT_TIMER_BASE STM32_TIM5_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM5
# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN
# if CONFIG_STM32_TIM5
# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
# endif
#elif HRT_TIMER == 8
# define HRT_TIMER_BASE STM32_TIM8_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC
# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN
# if CONFIG_STM32_TIM8
# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6
# endif
#elif HRT_TIMER == 9
# define HRT_TIMER_BASE STM32_TIM9_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK
# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN
# if CONFIG_STM32_TIM9
# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
# endif
#elif HRT_TIMER == 10
# define HRT_TIMER_BASE STM32_TIM10_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP
# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN
# if CONFIG_STM32_TIM10
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
# endif
#elif HRT_TIMER == 11
# define HRT_TIMER_BASE STM32_TIM11_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM
# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN
# if CONFIG_STM32_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
#else
# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
#endif
/*
* HRT clock must be a multiple of 1MHz greater than 1MHz
*/
#if (HRT_TIMER_CLOCK % 1000000) != 0
# error HRT_TIMER_CLOCK must be a multiple of 1MHz
#endif
#if HRT_TIMER_CLOCK <= 1000000
# error HRT_TIMER_CLOCK must be greater than 1MHz
#endif
/*
* Minimum/maximum deadlines.
*
* These are suitable for use with a 16-bit timer/counter clocked
* at 1MHz. The high-resolution timer need only guarantee that it
* not wrap more than once in the 50ms period for absolute time to
* be consistently maintained.
*
* The minimum deadline must be such that the time taken between
* reading a time and writing a deadline to the timer cannot
* result in missing the deadline.
*/
#define HRT_INTERVAL_MIN 50
#define HRT_INTERVAL_MAX 50000
/*
* Period of the free-running counter, in microseconds.
*/
#define HRT_COUNTER_PERIOD 65536
/*
* Scaling factor(s) for the free-running counter; convert an input
* in counts to a time in microseconds.
*/
#define HRT_COUNTER_SCALE(_c) (_c)
/*
* Timer register accessors
*/
#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
#define rCR1 REG(STM32_GTIM_CR1_OFFSET)
#define rCR2 REG(STM32_GTIM_CR2_OFFSET)
#define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
#define rDIER REG(STM32_GTIM_DIER_OFFSET)
#define rSR REG(STM32_GTIM_SR_OFFSET)
#define rEGR REG(STM32_GTIM_EGR_OFFSET)
#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET)
#define rCCER REG(STM32_GTIM_CCER_OFFSET)
#define rCNT REG(STM32_GTIM_CNT_OFFSET)
#define rPSC REG(STM32_GTIM_PSC_OFFSET)
#define rARR REG(STM32_GTIM_ARR_OFFSET)
#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET)
#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET)
#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET)
#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET)
#define rDCR REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
/*
* Specific registers and bits used by HRT sub-functions
*/
#if HRT_TIMER_CHANNEL == 1
# define rCCR_HRT rCCR1 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 2
# define rCCR_HRT rCCR2 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 3
# define rCCR_HRT rCCR3 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 4
# define rCCR_HRT rCCR4 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */
#else
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
#endif
/*
* Queue of callout entries.
*/
static struct sq_queue_s callout_queue;
/* latency baseline (last compare value applied) */
static uint16_t latency_baseline;
/* timer count at interrupt (for latency purposes) */
static uint16_t latency_actual;
/* latency histogram */
#define LATENCY_BUCKET_COUNT 8
static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
/* timer-specific functions */
static void hrt_tim_init(void);
static int hrt_tim_isr(int irq, void *context);
static void hrt_latency_update(void);
/* callout list manipulation */
static void hrt_call_internal(struct hrt_call *entry,
hrt_abstime deadline,
hrt_abstime interval,
hrt_callout callout,
void *arg);
static void hrt_call_enter(struct hrt_call *entry);
static void hrt_call_reschedule(void);
static void hrt_call_invoke(void);
/*
* Specific registers and bits used by PPM sub-functions
*/
#ifdef CONFIG_HRT_PPM
# if HRT_PPM_CHANNEL == 1
# define rCCR_PPM rCCR1 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 1 /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
# elif HRT_PPM_CHANNEL == 2
# define rCCR_PPM rCCR2 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 2 /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
# elif HRT_PPM_CHANNEL == 3
# define rCCR_PPM rCCR3 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 1 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
# elif HRT_PPM_CHANNEL == 4
# define rCCR_PPM rCCR4 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 2 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
# else
# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
# endif
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */
/* decoded PPM buffer */
#define PPM_MAX_CHANNELS 12
uint16_t ppm_buffer[PPM_MAX_CHANNELS];
unsigned ppm_decoded_channels;
uint64_t ppm_last_valid_decode = 0;
/* PPM edge history */
uint16_t ppm_edge_history[32];
unsigned ppm_edge_next;
/* PPM pulse history */
uint16_t ppm_pulse_history[32];
unsigned ppm_pulse_next;
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
/* PPM decoder state machine */
struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
unsigned next_channel;
enum {
UNSYNCH = 0,
ARM,
ACTIVE,
INACTIVE
} phase;
} ppm;
static void hrt_ppm_decode(uint32_t status);
#else
/* disable the PPM configuration */
# define rCCR_PPM 0
# define DIER_PPM 0
# define SR_INT_PPM 0
# define SR_OVF_PPM 0
# define CCMR1_PPM 0
# define CCMR2_PPM 0
# define CCER_PPM 0
#endif /* CONFIG_HRT_PPM */
/*
* Initialise the timer we are going to use.
*
* We expect that we'll own one of the reduced-function STM32 general
* timers, and that we can use channel 1 in compare mode.
*/
static void
hrt_tim_init(void)
{
/* clock/power on our timer */
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
/* claim our interrupt vector */
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
/* disable and configure the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = DIER_HRT | DIER_PPM;
rCCER = 0; /* unlock CCMR* registers */
rCCMR1 = CCMR1_PPM;
rCCMR2 = CCMR2_PPM;
rCCER = CCER_PPM;
rDCR = 0;
/* configure the timer to free-run at 1MHz */
rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */
/* run the full span of the counter */
rARR = 0xffff;
/* set an initial capture a little ways off */
rCCR_HRT = 1000;
/* generate an update event; reloads the counter, all registers */
rEGR = GTIM_EGR_UG;
/* enable the timer */
rCR1 = GTIM_CR1_CEN;
/* enable interrupts */
up_enable_irq(HRT_TIMER_VECTOR);
}
#ifdef CONFIG_HRT_PPM
/*
* Handle the PPM decoder state machine.
*/
static void
hrt_ppm_decode(uint32_t status)
{
uint16_t count = rCCR_PPM;
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (status & SR_OVF_PPM)
goto error;
/* how long since the last edge? */
width = count - ppm.last_edge;
ppm.last_edge = count;
ppm_edge_history[ppm_edge_next++] = width;
if (ppm_edge_next >= 32)
ppm_edge_next = 0;
/*
* if this looks like a start pulse, then push the last set of values
* and reset the state machine
*/
if (width >= PPM_MIN_START) {
/* export the last set of samples if we got something sensible */
if (ppm.next_channel > 4) {
for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_decoded_channels = i;
ppm_last_valid_decode = hrt_absolute_time();
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
return;
}
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
return;
case ARM:
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = count;
ppm.phase = INACTIVE;
return;
case INACTIVE:
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
return;
case ACTIVE:
/* determine the interval from the last mark */
interval = count - ppm.last_mark;
ppm.last_mark = count;
ppm_pulse_history[ppm_pulse_next++] = interval;
if (ppm_pulse_next >= 32)
ppm_pulse_next = 0;
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
goto error;
/* if we have room to store the value, do so */
if (ppm.next_channel < PPM_MAX_CHANNELS)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
return;
}
/* the state machine is corrupted; reset it */
error:
/* we don't like the state of the decoder, reset it and try again */
ppm.phase = UNSYNCH;
ppm_decoded_channels = 0;
}
#endif /* CONFIG_HRT_PPM */
/*
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
static int
hrt_tim_isr(int irq, void *context)
{
uint32_t status;
/* grab the timer for latency tracking purposes */
latency_actual = rCNT;
/* copy interrupt status */
status = rSR;
/* ack the interrupts we just read */
rSR = ~status;
#ifdef CONFIG_HRT_PPM
/* was this a PPM edge? */
if (status & (SR_INT_PPM | SR_OVF_PPM))
hrt_ppm_decode(status);
#endif
/* was this a timer tick? */
if (status & SR_INT_HRT) {
/* do latency calculations */
hrt_latency_update();
/* run any callouts that have met their deadline */
hrt_call_invoke();
/* and schedule the next interrupt */
hrt_call_reschedule();
}
return OK;
}
/*
* Fetch a never-wrapping absolute time value in microseconds from
* some arbitrary epoch shortly after system start.
*/
hrt_abstime
hrt_absolute_time(void)
{
hrt_abstime abstime;
uint32_t count;
uint32_t flags;
/*
* Counter state. Marked volatile as they may change
* inside this routine but outside the irqsave/restore
* pair. Discourage the compiler from moving loads/stores
* to these outside of the protected range.
*/
static volatile hrt_abstime base_time;
static volatile uint32_t last_count;
/* prevent re-entry */
flags = irqsave();
/* get the current counter value */
count = rCNT;
/*
* Determine whether the counter has wrapped since the
* last time we're called.
*
* This simple test is sufficient due to the guarantee that
* we are always called at least once per counter period.
*/
if (count < last_count)
base_time += HRT_COUNTER_PERIOD;
/* save the count for next time */
last_count = count;
/* compute the current time */
abstime = HRT_COUNTER_SCALE(base_time + count);
irqrestore(flags);
return abstime;
}
/*
* Convert a timespec to absolute time
*/
hrt_abstime
ts_to_abstime(struct timespec *ts)
{
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
return result;
}
/*
* Convert absolute time to a timespec.
*/
void
abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
{
ts->tv_sec = abstime / 1000000;
abstime -= ts->tv_sec * 1000000;
ts->tv_nsec = abstime * 1000;
}
/*
* Initalise the high-resolution timing module.
*/
void
hrt_init(void)
{
sq_init(&callout_queue);
hrt_tim_init();
#ifdef CONFIG_HRT_PPM
/* configure the PPM input pin */
stm32_configgpio(GPIO_PPM_IN);
#endif
}
/*
* Call callout(arg) after interval has elapsed.
*/
void
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
0,
callout,
arg);
}
/*
* Call callout(arg) at calltime.
*/
void
hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
hrt_call_internal(entry, calltime, 0, callout, arg);
}
/*
* Call callout(arg) every period.
*/
void
hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
interval,
callout,
arg);
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
irqrestore(flags);
}
/*
* If this returns true, the call has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
*/
bool
hrt_called(struct hrt_call *entry)
{
return (entry->deadline == 0);
}
/*
* Remove the entry from the callout list.
*/
void
hrt_cancel(struct hrt_call *entry)
{
irqstate_t flags = irqsave();
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
/* if this is a periodic call being removed by the callout, prevent it from
* being re-entered when the callout returns.
*/
entry->period = 0;
irqrestore(flags);
}
static void
hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == NULL) || (entry->deadline < call->deadline)) {
sq_addfirst(&entry->link, &callout_queue);
//lldbg("call enter at head, reschedule\n");
/* we changed the next deadline, reschedule the timer event */
hrt_call_reschedule();
} else {
do {
next = (struct hrt_call *)sq_next(&call->link);
if ((next == NULL) || (entry->deadline < next->deadline)) {
//lldbg("call enter after head\n");
sq_addafter(&call->link, &entry->link, &callout_queue);
break;
}
} while ((call = next) != NULL);
}
//lldbg("scheduled\n");
}
static void
hrt_call_invoke(void)
{
struct hrt_call *call;
hrt_abstime deadline;
while (true) {
/* get the current time */
hrt_abstime now = hrt_absolute_time();
call = (struct hrt_call *)sq_peek(&callout_queue);
if (call == NULL)
break;
if (call->deadline > now)
break;
sq_rem(&call->link, &callout_queue);
//lldbg("call pop\n");
/* save the intended deadline for periodic calls */
deadline = call->deadline;
/* zero the deadline, as the call has occurred */
call->deadline = 0;
/* invoke the callout (if there is one) */
if (call->callout) {
//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
call->callout(call->arg);
}
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
call->deadline = deadline + call->period;
hrt_call_enter(call);
}
}
}
/*
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
*/
static void
hrt_call_reschedule()
{
hrt_abstime now = hrt_absolute_time();
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
/*
* Determine what the next deadline will be.
*
* Note that we ensure that this will be within the counter
* period, so that when we truncate all but the low 16 bits
* the next time the compare matches it will be the deadline
* we want.
*
* It is important for accurate timekeeping that the compare
* interrupt fires sufficiently often that the base_time update in
* hrt_absolute_time runs at least once per timer period.
*/
if (next != NULL) {
//lldbg("entry in queue\n");
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
//lldbg("pre-expired\n");
/* set a minimal deadline so that we call ASAP */
deadline = now + HRT_INTERVAL_MIN;
} else if (next->deadline < deadline) {
//lldbg("due soon\n");
deadline = next->deadline;
}
}
//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
/* set the new compare value and remember it for latency tracking */
rCCR_HRT = latency_baseline = deadline & 0xffff;
}
static void
hrt_latency_update(void)
{
uint16_t latency = latency_actual - latency_baseline;
unsigned index;
/* bounded buckets */
for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
if (latency <= latency_buckets[index]) {
latency_counters[index]++;
return;
}
}
/* catch-all at the end */
latency_counters[index]++;
}
#endif /* CONFIG_HRT_TIMER */
nuttx/configs/px4fmu/src/up_nsh.c
-280
View File
@@ -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;
}
nuttx/configs/px4fmu/src/up_pwm_servo.c
-346
View File
@@ -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;
}
}
nuttx/configs/px4fmu/src/up_spi.c
-192
View File
@@ -1,192 +0,0 @@
/************************************************************************************
* configs/px4fmu/src/up_spi.c
* arch/arm/src/board/up_spi.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 <nuttx/spi.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "chip.h"
#include "stm32_internal.h"
#include "px4fmu-internal.h"
#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3)
/************************************************************************************
* Definitions
************************************************************************************/
/* Enables debug output from this file (needs CONFIG_DEBUG too) */
#undef SPI_DEBUG /* Define to enable debug */
#undef SPI_VERBOSE /* Define to enable verbose debug */
#ifdef SPI_DEBUG
# define spidbg lldbg
# ifdef SPI_VERBOSE
# define spivdbg lldbg
# else
# define spivdbg(x...)
# endif
#else
# undef SPI_VERBOSE
# define spidbg(x...)
# define spivdbg(x...)
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_spiinitialize
*
* Description:
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
*
************************************************************************************/
void weak_function stm32_spiinitialize(void)
{
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL);
stm32_configgpio(GPIO_SPI_CS_MPU);
stm32_configgpio(GPIO_SPI_CS_SDCARD);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
}
/****************************************************************************
* Name: stm32_spi1/2/3select and stm32_spi1/2/3status
*
* Description:
* The external functions, stm32_spi1/2/3select and stm32_spi1/2/3status must be
* provided by board-specific logic. They are implementations of the select
* and status methods of the SPI interface defined by struct spi_ops_s (see
* include/nuttx/spi.h). All other methods (including up_spiinitialize())
* are provided by common STM32 logic. To use this common SPI logic on your
* board:
*
* 1. Provide logic in stm32_boardinitialize() to configure SPI chip select
* pins.
* 2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions in your
* board-specific logic. These functions will perform chip selection and
* status operations using GPIOs in the way your board is configured.
* 3. Add a calls to up_spiinitialize() in your low level application
* initialization logic
* 4. The handle returned by up_spiinitialize() may then be used to bind the
* SPI driver to higher level logic (e.g., calling
* mmcsd_spislotinitialize(), for example, will bind the SPI driver to
* the SPI MMC/SD driver).
*
****************************************************************************/
#ifdef CONFIG_STM32_SPI1
void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
/* SPI select is active low, so write !selected to select the device */
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
break;
case PX4_SPIDEV_ACCEL:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
default:
spidbg("devid: %d - unexpected\n", devid);
break;
}
}
uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
return SPI_STATUS_PRESENT;
}
#endif
#ifdef CONFIG_STM32_SPI3
void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
/* there can only be one device on this bus, so always select it */
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
}
uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
return SPI_STATUS_PRESENT;
}
#endif
#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 */
nuttx/configs/px4fmu/src/up_usbdev.c
-105
View File
@@ -1,105 +0,0 @@
/************************************************************************************
* configs/stm32f4discovery/src/up_usbdev.c
* arch/arm/src/board/up_boot.c
*
* Copyright (C) 2012 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 <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <nuttx/usb/usbdev.h>
#include <nuttx/usb/usbdev_trace.h>
#include "up_arch.h"
#include "stm32_internal.h"
#include "px4fmu-internal.h"
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_usbinitialize
*
* Description:
* Called to setup USB-related GPIO pins for the STM3210E-EVAL board.
*
************************************************************************************/
void stm32_usbinitialize(void)
{
/* The OTG FS has an internal soft pull-up */
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
#ifdef CONFIG_STM32_OTGFS
stm32_configgpio(GPIO_OTGFS_VBUS);
/* XXX We only support device mode
stm32_configgpio(GPIO_OTGFS_PWRON);
stm32_configgpio(GPIO_OTGFS_OVER);
*/
#endif
}
/************************************************************************************
* Name: stm32_usbsuspend
*
* Description:
* Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
* used. This function is called whenever the USB enters or leaves suspend mode.
* This is an opportunity for the board logic to shutdown clocks, power, etc.
* while the USB is suspended.
*
************************************************************************************/
void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
{
ulldbg("resume: %d\n", resume);
}