mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-12 20:27:19 +08:00
Merge branch 'master' into mc_mixer_fix
This commit is contained in:
Anton Babushkin
@@ -0,0 +1,12 @@
|
||||
{
|
||||
"board_id": 19,
|
||||
"magic": "AeroCore",
|
||||
"description": "Firmware for the Gumstix AeroCore board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "AEROCORE",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"git_identity": "",
|
||||
"board_revision": 0
|
||||
}
|
||||
@@ -6,7 +6,7 @@
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
fw_att_pos_estimator start
|
||||
ekf_att_pos_estimator start
|
||||
|
||||
#
|
||||
# Start attitude controller
|
||||
|
||||
@@ -11,6 +11,4 @@ then
|
||||
param set NAV_RTL_ALT 100
|
||||
param set NAV_RTL_LAND_T -1
|
||||
param set NAV_ACCEPT_RAD 50
|
||||
param set RC_SCALE_ROLL 1
|
||||
param set RC_SCALE_PITCH 1
|
||||
fi
|
||||
@@ -8,7 +8,7 @@ then
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
echo "Start sdlog2 at 50Hz"
|
||||
sdlog2 start -r 50 -a -b 8 -t
|
||||
sdlog2 start -r 50 -a -b 5 -t
|
||||
else
|
||||
echo "Start sdlog2 at 200Hz"
|
||||
sdlog2 start -r 200 -a -b 16 -t
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#
|
||||
|
||||
attitude_estimator_ekf start
|
||||
#ekf_att_pos_estimator start
|
||||
position_estimator_inav start
|
||||
|
||||
mc_att_control start
|
||||
|
||||
@@ -35,6 +35,12 @@ then
|
||||
param set MPC_TILTMAX_AIR 45.0
|
||||
param set MPC_TILTMAX_LND 15.0
|
||||
param set MPC_LAND_SPEED 1.0
|
||||
|
||||
param set PE_VELNE_NOISE 0.5
|
||||
param set PE_VELNE_NOISE 0.7
|
||||
param set PE_POSNE_NOISE 0.5
|
||||
param set PE_POSD_NOISE 1.0
|
||||
|
||||
fi
|
||||
|
||||
set PWM_RATE 400
|
||||
|
||||
@@ -3,16 +3,20 @@
|
||||
# USB MAVLink start
|
||||
#
|
||||
|
||||
echo "Starting MAVLink on this USB console"
|
||||
|
||||
mavlink start -r 10000 -d /dev/ttyACM0
|
||||
# Enable a number of interesting streams we want via USB
|
||||
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
|
||||
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
|
||||
usleep 100000
|
||||
|
||||
# Exit shell to make it available to MAVLink
|
||||
exit
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
#
|
||||
# Board-specific definitions for the Gumstix AeroCore
|
||||
#
|
||||
|
||||
#
|
||||
# Configure the toolchain
|
||||
#
|
||||
CONFIG_ARCH = CORTEXM4F
|
||||
CONFIG_BOARD = AEROCORE
|
||||
|
||||
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
|
||||
@@ -0,0 +1,125 @@
|
||||
#
|
||||
# Makefile for the AeroCore *default* configuration
|
||||
#
|
||||
|
||||
#
|
||||
# Use the configuration's ROMFS.
|
||||
#
|
||||
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
|
||||
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
MODULES += drivers/device
|
||||
MODULES += drivers/stm32
|
||||
MODULES += drivers/stm32/adc
|
||||
MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/aerocore
|
||||
MODULES += drivers/lsm303d
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/preflight_check
|
||||
MODULES += systemcmds/pwm
|
||||
MODULES += systemcmds/esc_calib
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/top
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/dumpfile
|
||||
|
||||
#
|
||||
# General system control
|
||||
#
|
||||
MODULES += modules/commander
|
||||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += modules/fw_att_pos_estimator
|
||||
MODULES += modules/position_estimator_inav
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/controllib
|
||||
MODULES += modules/uORB
|
||||
MODULES += modules/dataman
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
MODULES += lib/mathlib/math/filter
|
||||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
MODULES += lib/launchdetection
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
#MODULES += examples/math_demo
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/hello_sky
|
||||
MODULES += examples/px4_simple_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/daemon
|
||||
#MODULES += examples/px4_daemon_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
||||
#MODULES += examples/px4_mavlink_debug
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
|
||||
#MODULES += examples/fixedwing_control
|
||||
|
||||
# Hardware test
|
||||
#MODULES += examples/hwtest
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
# In general, these should move to modules over time.
|
||||
#
|
||||
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
|
||||
# to make the table a bit more readable.
|
||||
#
|
||||
define _B
|
||||
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
|
||||
endef
|
||||
|
||||
BUILTIN_COMMANDS := \
|
||||
$(call _B, hello, , 2048, hello_main) \
|
||||
$(call _B, i2c, , 2048, i2c_main)
|
||||
@@ -70,7 +70,7 @@ MODULES += modules/gpio_led
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/fw_att_pos_estimator
|
||||
MODULES += modules/ekf_att_pos_estimator
|
||||
MODULES += modules/position_estimator_inav
|
||||
#MODULES += examples/flow_position_estimator
|
||||
|
||||
|
||||
@@ -79,7 +79,7 @@ MODULES += modules/gpio_led
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += modules/fw_att_pos_estimator
|
||||
MODULES += modules/ekf_att_pos_estimator
|
||||
MODULES += modules/position_estimator_inav
|
||||
MODULES += examples/flow_position_estimator
|
||||
|
||||
|
||||
@@ -0,0 +1,263 @@
|
||||
/************************************************************************************
|
||||
* configs/aerocore/include/board.h
|
||||
* include/arch/board/board.h
|
||||
*
|
||||
* Copyright (C) 2009 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.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef __ARCH_BOARD_BOARD_H
|
||||
#define __ARCH_BOARD_BOARD_H
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#ifndef __ASSEMBLY__
|
||||
# include <stdint.h>
|
||||
#endif
|
||||
|
||||
#include <stm32.h>
|
||||
|
||||
/************************************************************************************
|
||||
* Definitions
|
||||
************************************************************************************/
|
||||
|
||||
/* Clocking *************************************************************************/
|
||||
/* The AeroCore uses a 24MHz crystal connected to the HSE.
|
||||
*
|
||||
* This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
|
||||
* System Clock source : PLL (HSE)
|
||||
* SYSCLK(Hz) : 168000000 Determined by PLL configuration
|
||||
* HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE)
|
||||
* AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE)
|
||||
* APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1)
|
||||
* APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2)
|
||||
* HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL)
|
||||
* PLLM : 24 (STM32_PLLCFG_PLLM)
|
||||
* PLLN : 336 (STM32_PLLCFG_PLLN)
|
||||
* PLLP : 2 (STM32_PLLCFG_PLLP)
|
||||
* PLLQ : 7 (STM32_PLLCFG_PPQ)
|
||||
* Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK
|
||||
* Flash Latency(WS) : 5
|
||||
* Prefetch Buffer : OFF
|
||||
* Instruction cache : ON
|
||||
* Data cache : ON
|
||||
* Require 48MHz for USB OTG FS, : Enabled
|
||||
* SDIO and RNG clock
|
||||
*/
|
||||
|
||||
/* HSI - 16 MHz RC factory-trimmed
|
||||
* LSI - 32 KHz RC
|
||||
* HSE - On-board crystal frequency is 24MHz
|
||||
* LSE - not installed
|
||||
*/
|
||||
|
||||
#define STM32_BOARD_XTAL 24000000ul
|
||||
|
||||
#define STM32_HSI_FREQUENCY 16000000ul
|
||||
#define STM32_LSI_FREQUENCY 32000
|
||||
#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
|
||||
|
||||
/* Main PLL Configuration.
|
||||
*
|
||||
* PLL source is HSE
|
||||
* PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
|
||||
* = (24,000,000 / 24) * 336
|
||||
* = 336,000,000
|
||||
* SYSCLK = PLL_VCO / PLLP
|
||||
* = 336,000,000 / 2 = 168,000,000
|
||||
* USB OTG FS, SDIO and RNG Clock
|
||||
* = PLL_VCO / PLLQ
|
||||
* = 48,000,000
|
||||
*/
|
||||
|
||||
#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24)
|
||||
#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336)
|
||||
#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2
|
||||
#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7)
|
||||
|
||||
#define STM32_SYSCLK_FREQUENCY 168000000ul
|
||||
|
||||
/* AHB clock (HCLK) is SYSCLK (168MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */
|
||||
#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
|
||||
#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
|
||||
|
||||
/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */
|
||||
#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4)
|
||||
|
||||
/* Timers driven from APB1 will be twice PCLK1 */
|
||||
|
||||
#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
|
||||
/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */
|
||||
#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2)
|
||||
|
||||
/* Timers driven from APB2 will be twice PCLK2 */
|
||||
|
||||
#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
|
||||
/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
|
||||
* otherwise frequency is 2xAPBx.
|
||||
* Note: TIM1,8 are on APB2, others on APB1
|
||||
*/
|
||||
|
||||
#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY)
|
||||
|
||||
/* Alternate function pin selections ************************************************/
|
||||
|
||||
/*
|
||||
* UARTs.
|
||||
*/
|
||||
/* USART1 on PB[6,7]: GPS */
|
||||
#define GPIO_USART1_RX GPIO_USART1_RX_2
|
||||
#define GPIO_USART1_TX GPIO_USART1_TX_2
|
||||
|
||||
/* USART2 on PD[5,6]: J5 Breakout */
|
||||
#define GPIO_USART2_RX GPIO_USART2_RX_2
|
||||
#define GPIO_USART2_TX GPIO_USART2_TX_2
|
||||
#define GPIO_USART2_CTS 0 // unused
|
||||
#define GPIO_USART2_RTS 0 // unused
|
||||
|
||||
/* USART3 on PD[8,9]: to DuoVero UART2 */
|
||||
#define GPIO_USART3_RX GPIO_USART3_RX_3
|
||||
#define GPIO_USART3_TX GPIO_USART3_TX_3
|
||||
#define GPIO_USART3_CTS 0 // unused
|
||||
#define GPIO_USART3_RTS 0 // unused
|
||||
|
||||
/* UART7 on PE[78]: J7 Breakout */
|
||||
#define GPIO_UART7_RX GPIO_UART7_RX_1
|
||||
#define GPIO_UART7_TX GPIO_UART7_TX_1
|
||||
|
||||
/*
|
||||
* UART8 on PE[0-1]: System Console on Port C of USB (J7)
|
||||
* No alternate pin config
|
||||
*/
|
||||
|
||||
/* USART[1,6] require a RX DMA configuration */
|
||||
#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
|
||||
#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
|
||||
|
||||
/*
|
||||
* I2C
|
||||
*
|
||||
* The optional _GPIO configurations allow the I2C driver to manually
|
||||
* reset the bus to clear stuck slaves. They match the pin configuration,
|
||||
* but are normally-high GPIOs.
|
||||
*/
|
||||
|
||||
/* PB[10-11]: I2C2 is broken out on J9 header */
|
||||
#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1
|
||||
#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1
|
||||
#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
|
||||
#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
|
||||
|
||||
/*
|
||||
* SPI
|
||||
*/
|
||||
/* PA[4-7] SPI1 broken out on J12 */
|
||||
#define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */
|
||||
#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
|
||||
|
||||
/* PB[12-15]: SPI2 connected to DuoVero SPI1 */
|
||||
#define GPIO_SPI2_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */
|
||||
#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
|
||||
|
||||
/* PC[10-12]: SPI3 connected to onboard sensors */
|
||||
#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz)
|
||||
|
||||
/* PE[11-14]: SPI4 connected to FRAM */
|
||||
#define GPIO_SPI4_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */
|
||||
#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz)
|
||||
|
||||
/************************************************************************************
|
||||
* Public Data
|
||||
************************************************************************************/
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
#undef EXTERN
|
||||
#if defined(__cplusplus)
|
||||
#define EXTERN extern "C"
|
||||
extern "C" {
|
||||
#else
|
||||
#define EXTERN extern
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Public Function Prototypes
|
||||
************************************************************************************/
|
||||
/************************************************************************************
|
||||
* 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.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
EXTERN void stm32_boardinitialize(void);
|
||||
|
||||
#undef EXTERN
|
||||
#if defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
#endif /* __ARCH_BOARD_BOARD_H */
|
||||
+9
-16
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 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
|
||||
@@ -31,19 +31,12 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
/**
|
||||
* nsh_romfsetc.h
|
||||
*
|
||||
* This file is a stub for 'make export' purposes; the actual ROMFS
|
||||
* must be supplied by the library client.
|
||||
*/
|
||||
|
||||
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
|
||||
extern unsigned char romfs_img[];
|
||||
extern unsigned int romfs_img_len;
|
||||
@@ -0,0 +1,179 @@
|
||||
############################################################################
|
||||
# configs/aerocore/nsh/Make.defs
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
include ${TOPDIR}/.config
|
||||
include ${TOPDIR}/tools/Config.mk
|
||||
|
||||
#
|
||||
# We only support building with the ARM bare-metal toolchain from
|
||||
# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
|
||||
#
|
||||
CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI
|
||||
|
||||
include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
|
||||
|
||||
CC = $(CROSSDEV)gcc
|
||||
CXX = $(CROSSDEV)g++
|
||||
CPP = $(CROSSDEV)gcc -E
|
||||
LD = $(CROSSDEV)ld
|
||||
AR = $(CROSSDEV)ar rcs
|
||||
NM = $(CROSSDEV)nm
|
||||
OBJCOPY = $(CROSSDEV)objcopy
|
||||
OBJDUMP = $(CROSSDEV)objdump
|
||||
|
||||
MAXOPTIMIZATION = -O3
|
||||
ARCHCPUFLAGS = -mcpu=cortex-m4 \
|
||||
-mthumb \
|
||||
-march=armv7e-m \
|
||||
-mfpu=fpv4-sp-d16 \
|
||||
-mfloat-abi=hard
|
||||
|
||||
|
||||
# enable precise stack overflow tracking
|
||||
INSTRUMENTATIONDEFINES = -finstrument-functions \
|
||||
-ffixed-r10
|
||||
|
||||
# pull in *just* libm from the toolchain ... this is grody
|
||||
LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
|
||||
EXTRA_LIBS += $(LIBM)
|
||||
|
||||
# use our linker script
|
||||
LDSCRIPT = ld.script
|
||||
|
||||
ifeq ($(WINTOOL),y)
|
||||
# Windows-native toolchains
|
||||
DIRLINK = $(TOPDIR)/tools/copydir.sh
|
||||
DIRUNLINK = $(TOPDIR)/tools/unlink.sh
|
||||
MKDEP = $(TOPDIR)/tools/mknulldeps.sh
|
||||
ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
|
||||
ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
|
||||
ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
|
||||
else
|
||||
ifeq ($(PX4_WINTOOL),y)
|
||||
# Windows-native toolchains (MSYS)
|
||||
DIRLINK = $(TOPDIR)/tools/copydir.sh
|
||||
DIRUNLINK = $(TOPDIR)/tools/unlink.sh
|
||||
MKDEP = $(TOPDIR)/tools/mknulldeps.sh
|
||||
ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
|
||||
ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
|
||||
ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
|
||||
else
|
||||
# Linux/Cygwin-native toolchain
|
||||
MKDEP = $(TOPDIR)/tools/mkdeps.sh
|
||||
ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
|
||||
ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
|
||||
ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
|
||||
endif
|
||||
endif
|
||||
|
||||
# tool versions
|
||||
ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
|
||||
ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
|
||||
|
||||
# optimisation flags
|
||||
ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
|
||||
-fno-strict-aliasing \
|
||||
-fno-strength-reduce \
|
||||
-fomit-frame-pointer \
|
||||
-funsafe-math-optimizations \
|
||||
-fno-builtin-printf \
|
||||
-ffunction-sections \
|
||||
-fdata-sections
|
||||
|
||||
ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
|
||||
ARCHOPTIMIZATION += -g
|
||||
endif
|
||||
|
||||
ARCHCFLAGS = -std=gnu99
|
||||
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
|
||||
ARCHWARNINGS = -Wall \
|
||||
-Wextra \
|
||||
-Wdouble-promotion \
|
||||
-Wshadow \
|
||||
-Wfloat-equal \
|
||||
-Wframe-larger-than=1024 \
|
||||
-Wpointer-arith \
|
||||
-Wlogical-op \
|
||||
-Wmissing-declarations \
|
||||
-Wpacked \
|
||||
-Wno-unused-parameter
|
||||
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
|
||||
# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
|
||||
# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
|
||||
|
||||
ARCHCWARNINGS = $(ARCHWARNINGS) \
|
||||
-Wbad-function-cast \
|
||||
-Wstrict-prototypes \
|
||||
-Wold-style-declaration \
|
||||
-Wmissing-parameter-type \
|
||||
-Wmissing-prototypes \
|
||||
-Wnested-externs \
|
||||
-Wunsuffixed-float-constants
|
||||
ARCHWARNINGSXX = $(ARCHWARNINGS) \
|
||||
-Wno-psabi
|
||||
ARCHDEFINES =
|
||||
ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
|
||||
|
||||
# this seems to be the only way to add linker flags
|
||||
EXTRA_LIBS += --warn-common \
|
||||
--gc-sections
|
||||
|
||||
CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
|
||||
CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS)
|
||||
CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
|
||||
CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS)
|
||||
CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
|
||||
AFLAGS = $(CFLAGS) -D__ASSEMBLY__
|
||||
|
||||
NXFLATLDFLAGS1 = -r -d -warn-common
|
||||
NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
|
||||
LDNXFLATFLAGS = -e main -s 2048
|
||||
|
||||
OBJEXT = .o
|
||||
LIBEXT = .a
|
||||
EXEEXT =
|
||||
|
||||
|
||||
# produce partially-linked $1 from files in $2
|
||||
define PRELINK
|
||||
@echo "PRELINK: $1"
|
||||
$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
|
||||
endef
|
||||
|
||||
HOSTCC = gcc
|
||||
HOSTINCLUDES = -I.
|
||||
HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe
|
||||
HOSTLDFLAGS =
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
############################################################################
|
||||
# configs/aerocore/nsh/appconfig
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
# 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
|
||||
@@ -12,7 +14,7 @@
|
||||
# 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
|
||||
# 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.
|
||||
#
|
||||
@@ -31,12 +33,10 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Full attitude / position Extended Kalman Filter
|
||||
#
|
||||
# Path to example in apps/examples containing the user_start entry point
|
||||
|
||||
MODULE_COMMAND = att_pos_estimator_ekf
|
||||
CONFIGURED_APPS += examples/nsh
|
||||
|
||||
SRCS = kalman_main.cpp \
|
||||
KalmanNav.cpp \
|
||||
params.c
|
||||
# The NSH application library
|
||||
CONFIGURED_APPS += nshlib
|
||||
CONFIGURED_APPS += system/readline
|
||||
File diff suppressed because it is too large
Load Diff
Executable
+67
@@ -0,0 +1,67 @@
|
||||
#!/bin/bash
|
||||
# configs/aerocore/nsh/setenv.sh
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
|
||||
if [ "$_" = "$0" ] ; then
|
||||
echo "You must source this script, not run it!" 1>&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
WD=`pwd`
|
||||
if [ ! -x "setenv.sh" ]; then
|
||||
echo "This script must be executed from the top-level NuttX build directory"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [ -z "${PATH_ORIG}" ]; then
|
||||
export PATH_ORIG="${PATH}"
|
||||
fi
|
||||
|
||||
# This the Cygwin path to the location where I installed the RIDE
|
||||
# toolchain under windows. You will also have to edit this if you install
|
||||
# the RIDE toolchain in any other location
|
||||
#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
|
||||
|
||||
# This the Cygwin path to the location where I installed the CodeSourcery
|
||||
# toolchain under windows. You will also have to edit this if you install
|
||||
# the CodeSourcery toolchain in any other location
|
||||
export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
|
||||
|
||||
# This the Cygwin path to the location where I build the buildroot
|
||||
# toolchain.
|
||||
#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
|
||||
|
||||
# Add the path to the toolchain to the PATH varialble
|
||||
export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
|
||||
|
||||
echo "PATH : ${PATH}"
|
||||
@@ -0,0 +1,150 @@
|
||||
/****************************************************************************
|
||||
* configs/aerocore/common/ld.script
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
* 2) 16Kb of SRAM beginning at address 0x2001:c000
|
||||
* 3) 64Kb of SRAM beginning at address 0x2002:0000
|
||||
* 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
|
||||
*
|
||||
* When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
|
||||
* where the code expects to begin execution by jumping to the entry point in
|
||||
* the 0x0800:0000 address range.
|
||||
*
|
||||
* The first 0x4000 of flash is reserved for the bootloader.
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
|
||||
ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
|
||||
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
|
||||
EXTERN(_vectors) /* force the vectors to be included in the output */
|
||||
|
||||
/*
|
||||
* Ensure that abort() is present in the final object. The exception handling
|
||||
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
|
||||
*/
|
||||
EXTERN(abort)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
|
||||
/*
|
||||
* This is a hack to make the newlib libm __errno() call
|
||||
* use the NuttX get_errno_ptr() function.
|
||||
*/
|
||||
__errno = get_errno_ptr;
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Init functions (static constructors and the like)
|
||||
*/
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP(*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Construction data for parameters.
|
||||
*/
|
||||
__param ALIGN(4): {
|
||||
__param_start = ABSOLUTE(.);
|
||||
KEEP(*(__param*))
|
||||
__param_end = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
############################################################################
|
||||
# configs/aerocore/src/Makefile
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
-include $(TOPDIR)/Make.defs
|
||||
|
||||
CFLAGS += -I$(TOPDIR)/sched
|
||||
|
||||
ASRCS =
|
||||
AOBJS = $(ASRCS:.S=$(OBJEXT))
|
||||
|
||||
CSRCS = empty.c
|
||||
COBJS = $(CSRCS:.c=$(OBJEXT))
|
||||
|
||||
SRCS = $(ASRCS) $(CSRCS)
|
||||
OBJS = $(AOBJS) $(COBJS)
|
||||
|
||||
ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src
|
||||
ifeq ($(WINTOOL),y)
|
||||
CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
|
||||
-I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
|
||||
-I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
|
||||
else
|
||||
CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
|
||||
endif
|
||||
|
||||
all: libboard$(LIBEXT)
|
||||
|
||||
$(AOBJS): %$(OBJEXT): %.S
|
||||
$(call ASSEMBLE, $<, $@)
|
||||
|
||||
$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
|
||||
$(call COMPILE, $<, $@)
|
||||
|
||||
libboard$(LIBEXT): $(OBJS)
|
||||
$(call ARCHIVE, $@, $(OBJS))
|
||||
|
||||
.depend: Makefile $(SRCS)
|
||||
$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
|
||||
$(Q) touch $@
|
||||
|
||||
depend: .depend
|
||||
|
||||
clean:
|
||||
$(call DELFILE, libboard$(LIBEXT))
|
||||
$(call CLEAN)
|
||||
|
||||
distclean: clean
|
||||
$(call DELFILE, Make.dep)
|
||||
$(call DELFILE, .depend)
|
||||
|
||||
-include Make.dep
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
/*
|
||||
* There are no source files here, but libboard.a can't be empty, so
|
||||
* we have this empty source file to keep it company.
|
||||
*/
|
||||
@@ -0,0 +1,282 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 aerocore_init.c
|
||||
*
|
||||
* AeroCore-specific early startup code. This file implements the
|
||||
* nsh_archinitialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialisation.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* 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/gran.h>
|
||||
|
||||
#include <stm32.h>
|
||||
#include "board_config.h"
|
||||
#include <stm32_uart.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_led.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Configuration ************************************************************/
|
||||
|
||||
/* Debug ********************************************************************/
|
||||
|
||||
#ifdef CONFIG_CPP_HAVE_VARARGS
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message(...) lowsyslog(__VA_ARGS__)
|
||||
# else
|
||||
# define message(...) printf(__VA_ARGS__)
|
||||
# endif
|
||||
#else
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message lowsyslog
|
||||
# else
|
||||
# define message printf
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
#if defined(CONFIG_FAT_DMAMEMORY)
|
||||
# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
|
||||
# error microSD DMA support requires CONFIG_GRAN
|
||||
# endif
|
||||
|
||||
static GRAN_HANDLE dma_allocator;
|
||||
|
||||
/*
|
||||
* The DMA heap size constrains the total number of things that can be
|
||||
* ready to do DMA at a time.
|
||||
*
|
||||
* For example, FAT DMA depends on one sector-sized buffer per filesystem plus
|
||||
* one sector-sized buffer per file.
|
||||
*
|
||||
* We use a fundamental alignment / granule size of 64B; this is sufficient
|
||||
* to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
|
||||
*/
|
||||
static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
|
||||
static perf_counter_t g_dma_perf;
|
||||
|
||||
static void
|
||||
dma_alloc_init(void)
|
||||
{
|
||||
dma_allocator = gran_initialize(g_dma_heap,
|
||||
sizeof(g_dma_heap),
|
||||
7, /* 128B granule - must be > alignment (XXX bug?) */
|
||||
6); /* 64B alignment */
|
||||
if (dma_allocator == NULL) {
|
||||
message("[boot] DMA allocator setup FAILED");
|
||||
} else {
|
||||
g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* DMA-aware allocator stubs for the FAT filesystem.
|
||||
*/
|
||||
|
||||
__EXPORT void *fat_dma_alloc(size_t size);
|
||||
__EXPORT void fat_dma_free(FAR void *memory, size_t size);
|
||||
|
||||
void *
|
||||
fat_dma_alloc(size_t size)
|
||||
{
|
||||
perf_count(g_dma_perf);
|
||||
return gran_alloc(dma_allocator, size);
|
||||
}
|
||||
|
||||
void
|
||||
fat_dma_free(FAR void *memory, size_t size)
|
||||
{
|
||||
gran_free(dma_allocator, memory, size);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
# define dma_alloc_init()
|
||||
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* 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.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void
|
||||
stm32_boardinitialize(void)
|
||||
{
|
||||
/* configure SPI interfaces */
|
||||
stm32_spiinitialize();
|
||||
|
||||
/* configure LEDs */
|
||||
up_ledinit();
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: nsh_archinitialize
|
||||
*
|
||||
* Description:
|
||||
* Perform architecture specific initialization
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static struct spi_dev_s *spi3;
|
||||
static struct spi_dev_s *spi4;
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
__EXPORT int matherr(struct __exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#else
|
||||
__EXPORT int matherr(struct exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
__EXPORT int nsh_archinitialize(void)
|
||||
{
|
||||
|
||||
/* configure ADC pins */
|
||||
stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */
|
||||
stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */
|
||||
stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */
|
||||
stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */
|
||||
|
||||
/* configure the high-resolution time/callout interface */
|
||||
hrt_init();
|
||||
|
||||
/* configure the DMA allocator */
|
||||
dma_alloc_init();
|
||||
|
||||
/* configure CPU load estimation */
|
||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
||||
cpuload_initialize_once();
|
||||
#endif
|
||||
|
||||
/* set up the serial DMA polling */
|
||||
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);
|
||||
|
||||
/* initial LED state */
|
||||
drv_led_start();
|
||||
led_off(LED_AMBER);
|
||||
|
||||
/* Configure Sensors on SPI bus #3 */
|
||||
spi3 = up_spiinitialize(3);
|
||||
if (!spi3) {
|
||||
message("[boot] FAILED to initialize SPI port 3\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
/* Default: 1MHz, 8 bits, Mode 3 */
|
||||
SPI_SETFREQUENCY(spi3, 10000000);
|
||||
SPI_SETBITS(spi3, 8);
|
||||
SPI_SETMODE(spi3, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false);
|
||||
SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
|
||||
SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
|
||||
up_udelay(20);
|
||||
message("[boot] Initialized SPI port 3 (SENSORS)\n");
|
||||
|
||||
/* Configure FRAM on SPI bus #4 */
|
||||
spi4 = up_spiinitialize(4);
|
||||
if (!spi4) {
|
||||
message("[boot] FAILED to initialize SPI port 4\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
/* Default: ~10MHz, 8 bits, Mode 3 */
|
||||
SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
|
||||
SPI_SETBITS(spi4, 8);
|
||||
SPI_SETMODE(spi4, SPIDEV_MODE0);
|
||||
SPI_SELECT(spi4, SPIDEV_FLASH, false);
|
||||
message("[boot] Initialized SPI port 4 (FRAM)\n");
|
||||
|
||||
return OK;
|
||||
}
|
||||
+73
-69
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013 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
|
||||
@@ -32,86 +32,90 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file fw_att_pos_estimator_params.c
|
||||
* @file aerocore_led.c
|
||||
*
|
||||
* Parameters defined by the attitude and position estimator task
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* AeroCore LED backend.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "stm32.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/*
|
||||
* Estimator parameters, accessible via MAVLink
|
||||
*
|
||||
* Ideally we'd be able to get these from up_internal.h,
|
||||
* but since we want to be able to disable the NuttX use
|
||||
* of leds for system indication at will and there is no
|
||||
* separate switch, we need to build independent of the
|
||||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init();
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
extern void led_toggle(int led);
|
||||
__END_DECLS
|
||||
|
||||
/**
|
||||
* Velocity estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the velocity estimate from GPS.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
|
||||
__EXPORT void led_init()
|
||||
{
|
||||
stm32_configgpio(GPIO_LED0);
|
||||
stm32_configgpio(GPIO_LED1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Position estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the position estimate from GPS.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
|
||||
__EXPORT void led_on(int led)
|
||||
{
|
||||
switch (led) {
|
||||
case 0:
|
||||
stm32_gpiowrite(GPIO_LED0, true);
|
||||
break;
|
||||
|
||||
/**
|
||||
* Height estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the height estimate from the barometer.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
|
||||
case 1:
|
||||
stm32_gpiowrite(GPIO_LED1, true);
|
||||
break;
|
||||
|
||||
/**
|
||||
* Mag estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the magnetic field estimate from
|
||||
* the magnetometer.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
|
||||
default:
|
||||
warnx("LED ID not recognized\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* True airspeeed estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the airspeed estimate.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
|
||||
__EXPORT void led_off(int led)
|
||||
{
|
||||
switch (led) {
|
||||
case 0:
|
||||
stm32_gpiowrite(GPIO_LED0, false);
|
||||
break;
|
||||
|
||||
/**
|
||||
* GPS vs. barometric altitude update weight
|
||||
*
|
||||
* RE-CHECK this.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
|
||||
case 1:
|
||||
stm32_gpiowrite(GPIO_LED1, false);
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("LED ID not recognized\n");
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void led_toggle(int led)
|
||||
{
|
||||
switch (led) {
|
||||
case 0:
|
||||
if (stm32_gpioread(GPIO_LED0))
|
||||
stm32_gpiowrite(GPIO_LED0, false);
|
||||
else
|
||||
stm32_gpiowrite(GPIO_LED0, true);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (stm32_gpioread(GPIO_LED1))
|
||||
stm32_gpiowrite(GPIO_LED1, false);
|
||||
else
|
||||
stm32_gpiowrite(GPIO_LED1, true);
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("LED ID not recognized\n");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,117 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 aerocore_pwm_servo.c
|
||||
*
|
||||
* Configuration data for the stm32 pwm_servo driver.
|
||||
*
|
||||
* Note that these arrays must always be fully-sized.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <stm32.h>
|
||||
#include <stm32_gpio.h>
|
||||
#include <stm32_tim.h>
|
||||
|
||||
#include <drivers/stm32/drv_pwm_servo.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
|
||||
{
|
||||
.base = STM32_TIM1_BASE,
|
||||
.clock_register = STM32_RCC_APB2ENR,
|
||||
.clock_bit = RCC_APB2ENR_TIM1EN,
|
||||
.clock_freq = STM32_APB2_TIM1_CLKIN
|
||||
},
|
||||
{
|
||||
.base = STM32_TIM3_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM3EN,
|
||||
.clock_freq = STM32_APB1_TIM3_CLKIN
|
||||
}
|
||||
};
|
||||
|
||||
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH1OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 1,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH2OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH3OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH4OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 4,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM3_CH1OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 1,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM3_CH2OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM3_CH3OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1500,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM3_CH4OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 4,
|
||||
.default_value = 1500,
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,183 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 aerocore_spi.c
|
||||
*
|
||||
* Board-specific SPI functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* 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.h>
|
||||
#include "board_config.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void weak_function stm32_spiinitialize(void)
|
||||
{
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
stm32_configgpio(GPIO_SPI1_NSS);
|
||||
stm32_gpiowrite(GPIO_SPI1_NSS, 1);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
stm32_configgpio(GPIO_SPI2_NSS);
|
||||
stm32_gpiowrite(GPIO_SPI2_NSS, 1);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI3
|
||||
stm32_configgpio(GPIO_SPI_CS_GYRO);
|
||||
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
|
||||
stm32_configgpio(GPIO_SPI_CS_BARO);
|
||||
|
||||
/* De-activate all peripherals,
|
||||
* required for some peripheral
|
||||
* state machines
|
||||
*/
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
|
||||
stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
|
||||
stm32_configgpio(GPIO_EXTI_MAG_DRDY);
|
||||
stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI4
|
||||
stm32_configgpio(GPIO_SPI4_NSS);
|
||||
stm32_gpiowrite(GPIO_SPI4_NSS, 1);
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* there is only one device broken-out so select it */
|
||||
stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* there is only one device broken-out so select it */
|
||||
stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI3
|
||||
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* 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_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_ACCEL_MAG:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_BARO:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef CONFIG_STM32_SPI4
|
||||
__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* there can only be one device on this bus, so always select it */
|
||||
stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
/* FRAM is always present */
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,176 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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 board_config.h
|
||||
*
|
||||
* AeroCore internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/* these headers are not C++ safe */
|
||||
#include <stm32.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#define UDID_START 0x1FFF7A10
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************************************/
|
||||
|
||||
/* LEDs */
|
||||
#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9)
|
||||
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10)
|
||||
|
||||
/* Gyro */
|
||||
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0)
|
||||
#define SENSOR_BOARD_ROTATION_DEFAULT 3 /* SENSOR_BOARD_ROTATION_270_DEG */
|
||||
|
||||
/* Accel & Mag */
|
||||
#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1)
|
||||
#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2)
|
||||
|
||||
/* GPS */
|
||||
#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
|
||||
#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
|
||||
#define GPS_DEFAULT_UART_PORT "/dev/ttyS0"
|
||||
|
||||
/* SPI3--Sensors */
|
||||
#define PX4_SPI_BUS_SENSORS 3
|
||||
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
|
||||
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
|
||||
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
|
||||
|
||||
/* Nominal chip selects for devices on SPI bus #3 */
|
||||
#define PX4_SPIDEV_ACCEL_MAG 0
|
||||
#define PX4_SPIDEV_GYRO 1
|
||||
#define PX4_SPIDEV_BARO 2
|
||||
|
||||
/* User GPIOs broken out on J11 */
|
||||
#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
|
||||
#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
|
||||
#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
|
||||
#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12)
|
||||
#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
|
||||
#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
|
||||
#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15)
|
||||
#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)
|
||||
|
||||
#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
|
||||
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
|
||||
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
|
||||
#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
|
||||
#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
|
||||
#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
|
||||
#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
|
||||
#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
|
||||
|
||||
/* PWM
|
||||
*
|
||||
* Eight PWM outputs are configured.
|
||||
*
|
||||
* Pins:
|
||||
*
|
||||
* CH1 : PA8 : TIM1_CH1
|
||||
* CH2 : PA9 : TIM1_CH2
|
||||
* CH3 : PA10 : TIM1_CH3
|
||||
* CH4 : PA11 : TIM1_CH4
|
||||
* CH5 : PC6 : TIM3_CH1
|
||||
* CH6 : PC7 : TIM3_CH2
|
||||
* CH7 : PC8 : TIM3_CH3
|
||||
* CH8 : PC9 : TIM3_CH4
|
||||
*/
|
||||
#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1
|
||||
#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1
|
||||
#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1
|
||||
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1
|
||||
#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3
|
||||
#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3
|
||||
#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2
|
||||
#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 8 /* use timer 8 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
|
||||
|
||||
/* Tone Alarm (no onboard speaker )*/
|
||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* channel 1 */
|
||||
#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
|
||||
#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
|
||||
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public data
|
||||
****************************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void stm32_spiinitialize(void);
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
||||
@@ -0,0 +1,8 @@
|
||||
#
|
||||
# Board-specific startup code for the AeroCore
|
||||
#
|
||||
|
||||
SRCS = aerocore_init.c \
|
||||
aerocore_pwm_servo.c \
|
||||
aerocore_spi.c \
|
||||
aerocore_led.c
|
||||
@@ -85,6 +85,8 @@ __BEGIN_DECLS
|
||||
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
|
||||
|
||||
#define PX4_SPI_BUS_SENSORS 1
|
||||
|
||||
/*
|
||||
* Use these in place of the spi_dev_e enumeration to
|
||||
* select a specific SPI device on SPI1
|
||||
|
||||
@@ -107,6 +107,8 @@ __BEGIN_DECLS
|
||||
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
|
||||
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
|
||||
|
||||
#define PX4_SPI_BUS_SENSORS 1
|
||||
|
||||
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
|
||||
#define PX4_SPIDEV_GYRO 1
|
||||
#define PX4_SPIDEV_ACCEL_MAG 2
|
||||
|
||||
@@ -94,6 +94,14 @@
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
/*
|
||||
* AeroCore GPIO numbers and configuration.
|
||||
*
|
||||
*/
|
||||
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
|
||||
/* no GPIO driver on the PX4IOv1 board */
|
||||
#endif
|
||||
@@ -146,4 +154,4 @@
|
||||
|
||||
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
|
||||
|
||||
#endif /* _DRV_GPIO_H */
|
||||
#endif /* _DRV_GPIO_H */
|
||||
|
||||
@@ -43,10 +43,14 @@
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
#ifndef GPS_DEFAULT_UART_PORT
|
||||
#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
|
||||
#endif
|
||||
|
||||
#define GPS_DEVICE_PATH "/dev/gps"
|
||||
|
||||
|
||||
+13
-4
@@ -63,6 +63,8 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include "ubx.h"
|
||||
#include "mtk.h"
|
||||
|
||||
@@ -276,14 +278,14 @@ GPS::task_main()
|
||||
_report.timestamp_position = hrt_absolute_time();
|
||||
_report.lat = (int32_t)47.378301e7f;
|
||||
_report.lon = (int32_t)8.538777e7f;
|
||||
_report.alt = (int32_t)400e3f;
|
||||
_report.alt = (int32_t)1200e3f;
|
||||
_report.timestamp_variance = hrt_absolute_time();
|
||||
_report.s_variance_m_s = 10.0f;
|
||||
_report.p_variance_m = 10.0f;
|
||||
_report.c_variance_rad = 0.1f;
|
||||
_report.fix_type = 3;
|
||||
_report.eph_m = 3.0f;
|
||||
_report.epv_m = 7.0f;
|
||||
_report.eph_m = 0.9f;
|
||||
_report.epv_m = 1.8f;
|
||||
_report.timestamp_velocity = hrt_absolute_time();
|
||||
_report.vel_n_m_s = 0.0f;
|
||||
_report.vel_e_m_s = 0.0f;
|
||||
@@ -421,7 +423,14 @@ GPS::task_main()
|
||||
void
|
||||
GPS::cmd_reset()
|
||||
{
|
||||
//XXX add reset?
|
||||
#ifdef GPIO_GPS_NRESET
|
||||
warnx("Toggling GPS reset pin");
|
||||
stm32_configgpio(GPIO_GPS_NRESET);
|
||||
stm32_gpiowrite(GPIO_GPS_NRESET, 0);
|
||||
usleep(100);
|
||||
stm32_gpiowrite(GPIO_GPS_NRESET, 1);
|
||||
warnx("Toggled GPS reset pin");
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@@ -34,6 +34,9 @@
|
||||
/**
|
||||
* @file l3gd20.cpp
|
||||
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
|
||||
*
|
||||
* Note: With the exception of the self-test feature, the ST L3G4200D is
|
||||
* also supported by this driver.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
@@ -89,9 +92,11 @@ static const int ERROR = -1;
|
||||
#define ADDR_WHO_AM_I 0x0F
|
||||
#define WHO_I_AM_H 0xD7
|
||||
#define WHO_I_AM 0xD4
|
||||
#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
|
||||
|
||||
#define ADDR_CTRL_REG1 0x20
|
||||
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
|
||||
|
||||
/* keep lowpass low to avoid noise issues */
|
||||
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
@@ -166,9 +171,14 @@ static const int ERROR = -1;
|
||||
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
|
||||
|
||||
#define L3GD20_DEFAULT_RATE 760
|
||||
#define L3G4200D_DEFAULT_RATE 800
|
||||
#define L3GD20_DEFAULT_RANGE_DPS 2000
|
||||
#define L3GD20_DEFAULT_FILTER_FREQ 30
|
||||
|
||||
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
|
||||
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
|
||||
#endif
|
||||
|
||||
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
|
||||
|
||||
class L3GD20 : public device::SPI
|
||||
@@ -216,6 +226,9 @@ private:
|
||||
math::LowPassFilter2p _gyro_filter_y;
|
||||
math::LowPassFilter2p _gyro_filter_z;
|
||||
|
||||
/* true if an L3G4200D is detected */
|
||||
bool _is_l3g4200d;
|
||||
|
||||
/**
|
||||
* Start automatic measurement.
|
||||
*/
|
||||
@@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
|
||||
_gyro_topic(-1),
|
||||
_class_instance(-1),
|
||||
_current_rate(0),
|
||||
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
|
||||
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
|
||||
_read(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
|
||||
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
|
||||
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
|
||||
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
|
||||
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_is_l3g4200d(false)
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
@@ -413,14 +427,7 @@ L3GD20::probe()
|
||||
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
|
||||
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
|
||||
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
|
||||
#else
|
||||
#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
#endif
|
||||
|
||||
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
|
||||
success = true;
|
||||
}
|
||||
|
||||
@@ -430,6 +437,13 @@ L3GD20::probe()
|
||||
success = true;
|
||||
}
|
||||
|
||||
/* Detect the L3G4200D used on AeroCore */
|
||||
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
|
||||
_is_l3g4200d = true;
|
||||
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
|
||||
success = true;
|
||||
}
|
||||
|
||||
if (success)
|
||||
return OK;
|
||||
|
||||
@@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT:
|
||||
if (_is_l3g4200d) {
|
||||
return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
|
||||
}
|
||||
return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
@@ -683,23 +700,26 @@ L3GD20::set_samplerate(unsigned frequency)
|
||||
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
|
||||
|
||||
if (frequency == 0)
|
||||
frequency = 760;
|
||||
frequency = _is_l3g4200d ? 800 : 760;
|
||||
|
||||
/* use limits good for H or non-H models */
|
||||
/*
|
||||
* Use limits good for H or non-H models. Rates are slightly different
|
||||
* for L3G4200D part but register settings are the same.
|
||||
*/
|
||||
if (frequency <= 100) {
|
||||
_current_rate = 95;
|
||||
_current_rate = _is_l3g4200d ? 100 : 95;
|
||||
bits |= RATE_95HZ_LP_25HZ;
|
||||
|
||||
} else if (frequency <= 200) {
|
||||
_current_rate = 190;
|
||||
_current_rate = _is_l3g4200d ? 200 : 190;
|
||||
bits |= RATE_190HZ_LP_50HZ;
|
||||
|
||||
} else if (frequency <= 400) {
|
||||
_current_rate = 380;
|
||||
_current_rate = _is_l3g4200d ? 400 : 380;
|
||||
bits |= RATE_380HZ_LP_50HZ;
|
||||
|
||||
} else if (frequency <= 800) {
|
||||
_current_rate = 760;
|
||||
_current_rate = _is_l3g4200d ? 800 : 760;
|
||||
bits |= RATE_760HZ_LP_50HZ;
|
||||
} else {
|
||||
return -EINVAL;
|
||||
@@ -772,7 +792,7 @@ L3GD20::reset()
|
||||
* callback fast enough to not miss data. */
|
||||
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
|
||||
|
||||
set_samplerate(0); // 760Hz
|
||||
set_samplerate(0); // 760Hz or 800Hz
|
||||
set_range(L3GD20_DEFAULT_RANGE_DPS);
|
||||
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
|
||||
|
||||
@@ -971,7 +991,7 @@ start()
|
||||
errx(0, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
|
||||
g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
||||
@@ -1793,7 +1793,7 @@ start()
|
||||
errx(0, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
|
||||
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
warnx("failed instantiating LSM303D obj");
|
||||
|
||||
@@ -117,7 +117,7 @@ private:
|
||||
device::Device *
|
||||
MS5611_spi_interface(ms5611::prom_u &prom_buf)
|
||||
{
|
||||
return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
|
||||
return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
|
||||
}
|
||||
|
||||
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
|
||||
|
||||
@@ -92,6 +92,7 @@ public:
|
||||
MODE_2PWM,
|
||||
MODE_4PWM,
|
||||
MODE_6PWM,
|
||||
MODE_8PWM,
|
||||
};
|
||||
PX4FMU();
|
||||
virtual ~PX4FMU();
|
||||
@@ -113,6 +114,9 @@ private:
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
static const unsigned _max_actuators = 6;
|
||||
#endif
|
||||
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
|
||||
static const unsigned _max_actuators = 8;
|
||||
#endif
|
||||
|
||||
Mode _mode;
|
||||
unsigned _pwm_default_rate;
|
||||
@@ -203,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
|
||||
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
|
||||
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
|
||||
#endif
|
||||
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
|
||||
/* AeroCore breaks out User GPIOs on J11 */
|
||||
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
|
||||
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
|
||||
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
|
||||
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
|
||||
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
|
||||
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0},
|
||||
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0},
|
||||
{GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0},
|
||||
{GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0},
|
||||
{GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0},
|
||||
{GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0},
|
||||
#endif
|
||||
};
|
||||
|
||||
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
|
||||
@@ -382,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
|
||||
|
||||
break;
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
|
||||
debug("MODE_8PWM");
|
||||
/* default output rates */
|
||||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
|
||||
/* XXX magic numbers */
|
||||
up_pwm_servo_init(0xff);
|
||||
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
|
||||
break;
|
||||
#endif
|
||||
|
||||
case MODE_NONE:
|
||||
debug("MODE_NONE");
|
||||
|
||||
@@ -602,6 +634,9 @@ PX4FMU::task_main()
|
||||
num_outputs = 6;
|
||||
break;
|
||||
|
||||
case MODE_8PWM:
|
||||
num_outputs = 8;
|
||||
break;
|
||||
default:
|
||||
num_outputs = 0;
|
||||
break;
|
||||
@@ -757,6 +792,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
case MODE_2PWM:
|
||||
case MODE_4PWM:
|
||||
case MODE_6PWM:
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case MODE_8PWM:
|
||||
#endif
|
||||
ret = pwm_ioctl(filp, cmd, arg);
|
||||
break;
|
||||
|
||||
@@ -986,6 +1024,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case PWM_SERVO_SET(7):
|
||||
case PWM_SERVO_SET(6):
|
||||
if (_mode < MODE_8PWM) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case PWM_SERVO_SET(5):
|
||||
case PWM_SERVO_SET(4):
|
||||
if (_mode < MODE_6PWM) {
|
||||
@@ -1013,6 +1060,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
|
||||
break;
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case PWM_SERVO_GET(7):
|
||||
case PWM_SERVO_GET(6):
|
||||
if (_mode < MODE_8PWM) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case PWM_SERVO_GET(5):
|
||||
case PWM_SERVO_GET(4):
|
||||
if (_mode < MODE_6PWM) {
|
||||
@@ -1040,12 +1096,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
case PWM_SERVO_GET_RATEGROUP(3):
|
||||
case PWM_SERVO_GET_RATEGROUP(4):
|
||||
case PWM_SERVO_GET_RATEGROUP(5):
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case PWM_SERVO_GET_RATEGROUP(6):
|
||||
case PWM_SERVO_GET_RATEGROUP(7):
|
||||
#endif
|
||||
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_COUNT:
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
switch (_mode) {
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
case MODE_8PWM:
|
||||
*(unsigned *)arg = 8;
|
||||
break;
|
||||
#endif
|
||||
|
||||
case MODE_6PWM:
|
||||
*(unsigned *)arg = 6;
|
||||
break;
|
||||
@@ -1091,6 +1157,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
set_mode(MODE_6PWM);
|
||||
break;
|
||||
#endif
|
||||
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
|
||||
case 8:
|
||||
set_mode(MODE_8PWM);
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
@@ -1181,10 +1252,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
|
||||
unsigned count = len / 2;
|
||||
uint16_t values[6];
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
if (count > 8) {
|
||||
// we have at most 8 outputs
|
||||
count = 8;
|
||||
}
|
||||
#else
|
||||
if (count > 6) {
|
||||
// we have at most 6 outputs
|
||||
count = 6;
|
||||
}
|
||||
#endif
|
||||
|
||||
// allow for misaligned values
|
||||
memcpy(values, buffer, count * 2);
|
||||
@@ -1458,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
|
||||
#endif
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
servo_mode = PX4FMU::MODE_6PWM;
|
||||
#endif
|
||||
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
|
||||
servo_mode = PX4FMU::MODE_8PWM;
|
||||
#endif
|
||||
break;
|
||||
|
||||
@@ -1776,7 +1857,7 @@ fmu_main(int argc, char *argv[])
|
||||
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
|
||||
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
|
||||
#endif
|
||||
exit(1);
|
||||
|
||||
@@ -4,3 +4,5 @@
|
||||
|
||||
MODULE_COMMAND = fmu
|
||||
SRCS = fmu.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
@@ -44,3 +44,5 @@ SRCS = px4io.cpp \
|
||||
|
||||
# XXX prune to just get UART registers
|
||||
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
@@ -529,6 +529,11 @@ PX4IO::~PX4IO()
|
||||
if (_interface != nullptr)
|
||||
delete _interface;
|
||||
|
||||
/* deallocate perfs */
|
||||
perf_free(_perf_update);
|
||||
perf_free(_perf_write);
|
||||
perf_free(_perf_chan_count);
|
||||
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
@@ -1448,7 +1453,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
||||
/* we don't have the status bits, so input_source has to be set elsewhere */
|
||||
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
|
||||
|
||||
static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
|
||||
const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
|
||||
uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
|
||||
|
||||
/*
|
||||
@@ -1456,8 +1461,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
||||
*
|
||||
* This should be the common case (9 channel R/C control being a reasonable upper bound).
|
||||
*/
|
||||
input_rc.timestamp_publication = hrt_absolute_time();
|
||||
|
||||
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9);
|
||||
|
||||
if (ret != OK)
|
||||
@@ -1469,23 +1472,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
||||
*/
|
||||
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
|
||||
|
||||
if (channel_count != _rc_chan_count)
|
||||
/* limit the channel count */
|
||||
if (channel_count > RC_INPUT_MAX_CHANNELS) {
|
||||
channel_count = RC_INPUT_MAX_CHANNELS;
|
||||
}
|
||||
|
||||
/* count channel count changes to identify signal integrity issues */
|
||||
if (channel_count != _rc_chan_count) {
|
||||
perf_count(_perf_chan_count);
|
||||
}
|
||||
|
||||
_rc_chan_count = channel_count;
|
||||
|
||||
input_rc.timestamp_publication = hrt_absolute_time();
|
||||
|
||||
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
|
||||
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
|
||||
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
||||
input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
|
||||
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
|
||||
input_rc.channel_count = channel_count;
|
||||
|
||||
/* rc_lost has to be set before the call to this function */
|
||||
if (!input_rc.rc_lost && !input_rc.rc_failsafe)
|
||||
if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
|
||||
_rc_last_valid = input_rc.timestamp_publication;
|
||||
}
|
||||
|
||||
input_rc.timestamp_last_signal = _rc_last_valid;
|
||||
|
||||
/* FIELDS NOT SET HERE */
|
||||
/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
|
||||
|
||||
if (channel_count > 9) {
|
||||
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
|
||||
|
||||
@@ -1493,8 +1511,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
||||
return ret;
|
||||
}
|
||||
|
||||
input_rc.channel_count = channel_count;
|
||||
memcpy(input_rc.values, ®s[prolog], channel_count * 2);
|
||||
/* last thing set are the actual channel values as 16 bit values */
|
||||
for (unsigned i = 0; i < channel_count; i++) {
|
||||
input_rc.values[i] = regs[prolog + i];
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
|
||||
g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
|
||||
(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
|
||||
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
|
||||
#endif
|
||||
|
||||
if (g_adc == nullptr)
|
||||
errx(1, "couldn't allocate the ADC driver");
|
||||
|
||||
@@ -141,7 +141,7 @@
|
||||
# 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
|
||||
# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN
|
||||
# if CONFIG_STM32_TIM10
|
||||
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
|
||||
# endif
|
||||
@@ -150,7 +150,7 @@
|
||||
# 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
|
||||
# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN
|
||||
# if CONFIG_STM32_TIM11
|
||||
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
|
||||
# endif
|
||||
|
||||
@@ -39,3 +39,5 @@ MODULE_COMMAND = ex_fixedwing_control
|
||||
|
||||
SRCS = main.c \
|
||||
params.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
@@ -38,3 +38,5 @@
|
||||
MODULE_COMMAND = px4_daemon_app
|
||||
|
||||
SRCS = px4_daemon_app.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
@@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[])
|
||||
daemon_task = task_spawn_cmd("daemon",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
4096,
|
||||
2000,
|
||||
px4_daemon_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
|
||||
@@ -37,4 +37,6 @@
|
||||
|
||||
MODULE_COMMAND = px4_mavlink_debug
|
||||
|
||||
SRCS = px4_mavlink_debug.c
|
||||
SRCS = px4_mavlink_debug.c
|
||||
|
||||
MODULE_STACKSIZE = 2000
|
||||
|
||||
@@ -59,4 +59,8 @@
|
||||
#define HW_ARCH "PX4FMU_V2"
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_AEROCORE
|
||||
#define HW_ARCH "AEROCORE"
|
||||
#endif
|
||||
|
||||
#endif /* VERSION_H_ */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,194 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 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 KalmanNav.hpp
|
||||
*
|
||||
* kalman filter navigation code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
//#define MATRIX_ASSERT
|
||||
//#define VECTOR_ASSERT
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <poll.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/**
|
||||
* Kalman filter navigation class
|
||||
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
* Discrete-time extended Kalman filter
|
||||
*/
|
||||
class KalmanNav : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
KalmanNav(SuperBlock *parent, const char *name);
|
||||
|
||||
/**
|
||||
* Deconstuctor
|
||||
*/
|
||||
|
||||
virtual ~KalmanNav() {};
|
||||
|
||||
math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
|
||||
|
||||
/**
|
||||
* The main callback function for the class
|
||||
*/
|
||||
void update();
|
||||
|
||||
|
||||
/**
|
||||
* Publication update
|
||||
*/
|
||||
virtual void updatePublications();
|
||||
|
||||
/**
|
||||
* State prediction
|
||||
* Continuous, non-linear
|
||||
*/
|
||||
int predictState(float dt);
|
||||
|
||||
/**
|
||||
* State covariance prediction
|
||||
* Continuous, linear
|
||||
*/
|
||||
int predictStateCovariance(float dt);
|
||||
|
||||
/**
|
||||
* Attitude correction
|
||||
*/
|
||||
int correctAtt();
|
||||
|
||||
/**
|
||||
* Position correction
|
||||
*/
|
||||
int correctPos();
|
||||
|
||||
/**
|
||||
* Overloaded update parameters
|
||||
*/
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
// kalman filter
|
||||
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix<9,9> P; /**< state covariance matrix */
|
||||
math::Matrix<9,9> P0; /**< initial state covariance matrix */
|
||||
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
|
||||
// attitude
|
||||
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
// time stamps
|
||||
uint64_t _pubTimeStamp; /**< output data publication time stamp */
|
||||
uint64_t _predictTimeStamp; /**< prediction time stamp */
|
||||
uint64_t _attTimeStamp; /**< attitude correction time stamp */
|
||||
uint64_t _outTimeStamp; /**< output time stamp */
|
||||
// frame count
|
||||
uint16_t _navFrames; /**< navigation frames completed in output cycle */
|
||||
// miss counts
|
||||
uint16_t _miss; /**< number of times fast prediction loop missed */
|
||||
// accelerations
|
||||
float fN, fE, fD; /**< navigation frame acceleration */
|
||||
// states
|
||||
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
|
||||
float phi, theta, psi; /**< 3-2-1 euler angles */
|
||||
float vN, vE, vD; /**< navigation velocity, m/s */
|
||||
double lat, lon; /**< lat, lon radians */
|
||||
// parameters
|
||||
float alt; /**< altitude, meters */
|
||||
double lat0, lon0; /**< reference latitude and longitude */
|
||||
struct map_projection_reference_s ref; /**< local projection reference */
|
||||
float alt0; /**< refeerence altitude (ground height) */
|
||||
control::BlockParamFloat _vGyro; /**< gyro process noise */
|
||||
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
|
||||
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
|
||||
control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */
|
||||
control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */
|
||||
control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */
|
||||
control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */
|
||||
control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */
|
||||
control::BlockParamFloat _magDip; /**< magnetic inclination with level */
|
||||
control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */
|
||||
control::BlockParamFloat _g; /**< gravitational constant */
|
||||
control::BlockParamFloat _faultPos; /**< fault detection threshold for position */
|
||||
control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */
|
||||
// status
|
||||
bool _attitudeInitialized;
|
||||
bool _positionInitialized;
|
||||
uint16_t _attitudeInitCounter;
|
||||
// accessors
|
||||
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
|
||||
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
|
||||
};
|
||||
@@ -1,157 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: James Goppert
|
||||
*
|
||||
* 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 kalman_main.cpp
|
||||
* Combined attitude / position estimator.
|
||||
*
|
||||
* @author James Goppert
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
#include "KalmanNav.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int daemon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int kalman_demo_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int att_pos_estimator_ekf_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
warnx("already running");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
|
||||
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 30,
|
||||
8192,
|
||||
kalman_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
warnx("is running\n");
|
||||
exit(0);
|
||||
|
||||
} else {
|
||||
warnx("not started\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int kalman_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
warnx("starting");
|
||||
|
||||
using namespace math;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
KalmanNav nav(NULL, "KF");
|
||||
|
||||
while (!thread_should_exit) {
|
||||
nav.update();
|
||||
}
|
||||
|
||||
warnx("exiting.");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,8 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -34,9 +32,12 @@
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_main.c
|
||||
* @file attitude_estimator_ekf_main.cpp
|
||||
*
|
||||
* Extended Kalman Filter for Attitude Estimation.
|
||||
*
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
@@ -111,7 +112,7 @@ usage(const char *reason)
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
* to task_spawn_cmd().
|
||||
*/
|
||||
int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
@@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
|
||||
codegen/rtGetNaN.c \
|
||||
codegen/norm.c \
|
||||
codegen/cross.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user