dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into ekf_auto_mag_decl

This commit is contained in:
Anton Babushkin 2014-05-17 12:07:01 +02:00
parent e412bce1a1 32ae2dd1d0
commit 34184ff1fe
113 changed files with 6623 additions and 3827 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Images/aerocore.prototype Normal file
View File

@ -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
}

2
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

@ -6,7 +6,7 @@
# #
# Start the attitude and position estimator # Start the attitude and position estimator
# #
fw_att_pos_estimator start ekf_att_pos_estimator start
# #
# Start attitude controller # Start attitude controller

2
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -11,6 +11,4 @@ then
param set NAV_RTL_ALT 100 param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1 param set NAV_RTL_LAND_T -1
param set NAV_ACCEPT_RAD 50 param set NAV_ACCEPT_RAD 50
param set RC_SCALE_ROLL 1
param set RC_SCALE_PITCH 1
fi fi

2
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -8,7 +8,7 @@ then
if ver hwcmp PX4FMU_V1 if ver hwcmp PX4FMU_V1
then then
echo "Start sdlog2 at 50Hz" echo "Start sdlog2 at 50Hz"
sdlog2 start -r 50 -a -b 8 -t sdlog2 start -r 50 -a -b 4 -t
else else
echo "Start sdlog2 at 200Hz" echo "Start sdlog2 at 200Hz"
sdlog2 start -r 200 -a -b 16 -t sdlog2 start -r 200 -a -b 16 -t

1
ROMFS/px4fmu_common/init.d/rc.mc_apps
View File

@ -5,6 +5,7 @@
# #
attitude_estimator_ekf start attitude_estimator_ekf start
#ekf_att_pos_estimator start
position_estimator_inav start position_estimator_inav start
mc_att_control start mc_att_control start

6
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@ -35,6 +35,12 @@ then
param set MPC_TILTMAX_AIR 45.0 param set MPC_TILTMAX_AIR 45.0
param set MPC_TILTMAX_LND 15.0 param set MPC_TILTMAX_LND 15.0
param set MPC_LAND_SPEED 1.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 fi
set PWM_RATE 400 set PWM_RATE 400

10
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -3,16 +3,20 @@
# USB MAVLink start # USB MAVLink start
# #
echo "Starting MAVLink on this USB console"
mavlink start -r 10000 -d /dev/ttyACM0 mavlink start -r 10000 -d /dev/ttyACM0
# Enable a number of interesting streams we want via USB # Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
usleep 100000
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
usleep 100000
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 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 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 shell to make it available to MAVLink
exit exit

11
makefiles/board_aerocore.mk Normal file
View File

@ -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

125
makefiles/config_aerocore_default.mk Normal file
View File

@ -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/ekf_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)

2
makefiles/config_px4fmu-v1_default.mk
View File

@ -70,7 +70,7 @@ MODULES += modules/gpio_led
# Estimation modules (EKF/ SO3 / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/fw_att_pos_estimator MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator #MODULES += examples/flow_position_estimator

2
makefiles/config_px4fmu-v2_default.mk
View File

@ -79,7 +79,7 @@ MODULES += modules/gpio_led
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3 MODULES += modules/attitude_estimator_so3
MODULES += modules/fw_att_pos_estimator MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator

263
nuttx-configs/aerocore/include/board.h Normal file
View File

@ -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 */

25
src/modules/att_pos_estimator_ekf/params.c → nuttx-configs/aerocore/include/nsh_romfsimg.h
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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);*/ extern unsigned char romfs_img[];
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); extern unsigned int romfs_img_len;
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);

179
nuttx-configs/aerocore/nsh/Make.defs Normal file
View File

@ -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 =

18
src/modules/att_pos_estimator_ekf/module.mk → nuttx-configs/aerocore/nsh/appconfig
View File

@ -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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@ -12,7 +14,7 @@
# notice, this list of conditions and the following disclaimer in # notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the # the documentation and/or other materials provided with the
# distribution. # 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 # used to endorse or promote products derived from this software
# without specific prior written permission. # without specific prior written permission.
# #
@ -31,12 +33,10 @@
# #
############################################################################ ############################################################################
# # Path to example in apps/examples containing the user_start entry point
# Full attitude / position Extended Kalman Filter
#
MODULE_COMMAND = att_pos_estimator_ekf CONFIGURED_APPS += examples/nsh
SRCS = kalman_main.cpp \ # The NSH application library
KalmanNav.cpp \ CONFIGURED_APPS += nshlib
params.c CONFIGURED_APPS += system/readline

950
nuttx-configs/aerocore/nsh/defconfig Normal file
View File

@ -0,0 +1,950 @@
#
# Automatically generated file; DO NOT EDIT.
# Nuttx/ Configuration
#
CONFIG_NUTTX_NEWCONFIG=y
#
# Build Setup
#
# CONFIG_EXPERIMENTAL is not set
CONFIG_HOST_LINUX=y
# CONFIG_HOST_OSX is not set
# CONFIG_HOST_WINDOWS is not set
# CONFIG_HOST_OTHER is not set
#
# Build Configuration
#
CONFIG_APPS_DIR="../apps"
# CONFIG_BUILD_2PASS is not set
#
# Binary Output Formats
#
# CONFIG_RRLOAD_BINARY is not set
# CONFIG_INTELHEX_BINARY is not set
# CONFIG_MOTOROLA_SREC is not set
CONFIG_RAW_BINARY=y
#
# Customize Header Files
#
# CONFIG_ARCH_STDBOOL_H is not set
CONFIG_ARCH_MATH_H=y
# CONFIG_ARCH_FLOAT_H is not set
# CONFIG_ARCH_STDARG_H is not set
#
# Debug Options
#
# CONFIG_DEBUG is not set
# CONFIG_DEBUG_SYMBOLS is not set
#
# System Type
#
# CONFIG_ARCH_8051 is not set
CONFIG_ARCH_ARM=y
# CONFIG_ARCH_AVR is not set
# CONFIG_ARCH_HC is not set
# CONFIG_ARCH_MIPS is not set
# CONFIG_ARCH_RGMP is not set
# CONFIG_ARCH_SH is not set
# CONFIG_ARCH_SIM is not set
# CONFIG_ARCH_X86 is not set
# CONFIG_ARCH_Z16 is not set
# CONFIG_ARCH_Z80 is not set
CONFIG_ARCH="arm"
#
# ARM Options
#
# CONFIG_ARCH_CHIP_C5471 is not set
# CONFIG_ARCH_CHIP_CALYPSO is not set
# CONFIG_ARCH_CHIP_DM320 is not set
# CONFIG_ARCH_CHIP_IMX is not set
# CONFIG_ARCH_CHIP_KINETIS is not set
# CONFIG_ARCH_CHIP_KL is not set
# CONFIG_ARCH_CHIP_LM is not set
# CONFIG_ARCH_CHIP_LPC17XX is not set
# CONFIG_ARCH_CHIP_LPC214X is not set
# CONFIG_ARCH_CHIP_LPC2378 is not set
# CONFIG_ARCH_CHIP_LPC31XX is not set
# CONFIG_ARCH_CHIP_LPC43XX is not set
# CONFIG_ARCH_CHIP_NUC1XX is not set
# CONFIG_ARCH_CHIP_SAM34 is not set
CONFIG_ARCH_CHIP_STM32=y
# CONFIG_ARCH_CHIP_STR71X is not set
CONFIG_ARCH_CORTEXM4=y
CONFIG_ARCH_FAMILY="armv7-m"
CONFIG_ARCH_CHIP="stm32"
CONFIG_ARMV7M_USEBASEPRI=y
CONFIG_ARCH_HAVE_CMNVECTOR=y
CONFIG_ARMV7M_CMNVECTOR=y
CONFIG_ARCH_HAVE_FPU=y
CONFIG_ARCH_FPU=y
CONFIG_ARCH_HAVE_MPU=y
# CONFIG_ARMV7M_MPU is not set
#
# ARMV7M Configuration Options
#
# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
CONFIG_ARMV7M_STACKCHECK=y
CONFIG_SERIAL_TERMIOS=y
#
# STM32 Configuration Options
#
# CONFIG_ARCH_CHIP_STM32L151C6 is not set
# CONFIG_ARCH_CHIP_STM32L151C8 is not set
# CONFIG_ARCH_CHIP_STM32L151CB is not set
# CONFIG_ARCH_CHIP_STM32L151R6 is not set
# CONFIG_ARCH_CHIP_STM32L151R8 is not set
# CONFIG_ARCH_CHIP_STM32L151RB is not set
# CONFIG_ARCH_CHIP_STM32L151V6 is not set
# CONFIG_ARCH_CHIP_STM32L151V8 is not set
# CONFIG_ARCH_CHIP_STM32L151VB is not set
# CONFIG_ARCH_CHIP_STM32L152C6 is not set
# CONFIG_ARCH_CHIP_STM32L152C8 is not set
# CONFIG_ARCH_CHIP_STM32L152CB is not set
# CONFIG_ARCH_CHIP_STM32L152R6 is not set
# CONFIG_ARCH_CHIP_STM32L152R8 is not set
# CONFIG_ARCH_CHIP_STM32L152RB is not set
# CONFIG_ARCH_CHIP_STM32L152V6 is not set
# CONFIG_ARCH_CHIP_STM32L152V8 is not set
# CONFIG_ARCH_CHIP_STM32L152VB is not set
# CONFIG_ARCH_CHIP_STM32F100C8 is not set
# CONFIG_ARCH_CHIP_STM32F100CB is not set
# CONFIG_ARCH_CHIP_STM32F100R8 is not set
# CONFIG_ARCH_CHIP_STM32F100RB is not set
# CONFIG_ARCH_CHIP_STM32F100RC is not set
# CONFIG_ARCH_CHIP_STM32F100RD is not set
# CONFIG_ARCH_CHIP_STM32F100RE is not set
# CONFIG_ARCH_CHIP_STM32F100V8 is not set
# CONFIG_ARCH_CHIP_STM32F100VB is not set
# CONFIG_ARCH_CHIP_STM32F100VC is not set
# CONFIG_ARCH_CHIP_STM32F100VD is not set
# CONFIG_ARCH_CHIP_STM32F100VE is not set
# CONFIG_ARCH_CHIP_STM32F103C4 is not set
# CONFIG_ARCH_CHIP_STM32F103C8 is not set
# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
# CONFIG_ARCH_CHIP_STM32F107VC is not set
# CONFIG_ARCH_CHIP_STM32F207IG is not set
# CONFIG_ARCH_CHIP_STM32F302CB is not set
# CONFIG_ARCH_CHIP_STM32F302CC is not set
# CONFIG_ARCH_CHIP_STM32F302RB is not set
# CONFIG_ARCH_CHIP_STM32F302RC is not set
# CONFIG_ARCH_CHIP_STM32F302VB is not set
# CONFIG_ARCH_CHIP_STM32F302VC is not set
# CONFIG_ARCH_CHIP_STM32F303CB is not set
# CONFIG_ARCH_CHIP_STM32F303CC is not set
# CONFIG_ARCH_CHIP_STM32F303RB is not set
# CONFIG_ARCH_CHIP_STM32F303RC is not set
# CONFIG_ARCH_CHIP_STM32F303VB is not set
# CONFIG_ARCH_CHIP_STM32F303VC is not set
# CONFIG_ARCH_CHIP_STM32F405RG is not set
# CONFIG_ARCH_CHIP_STM32F405VG is not set
# CONFIG_ARCH_CHIP_STM32F405ZG is not set
# CONFIG_ARCH_CHIP_STM32F407VE is not set
# CONFIG_ARCH_CHIP_STM32F407VG is not set
# CONFIG_ARCH_CHIP_STM32F407ZE is not set
# CONFIG_ARCH_CHIP_STM32F407ZG is not set
# CONFIG_ARCH_CHIP_STM32F407IE is not set
# CONFIG_ARCH_CHIP_STM32F407IG is not set
CONFIG_ARCH_CHIP_STM32F427V=y
# CONFIG_ARCH_CHIP_STM32F427Z is not set
# CONFIG_ARCH_CHIP_STM32F427I is not set
# CONFIG_STM32_STM32L15XX is not set
# CONFIG_STM32_ENERGYLITE is not set
# CONFIG_STM32_STM32F10XX is not set
# CONFIG_STM32_VALUELINE is not set
# CONFIG_STM32_CONNECTIVITYLINE is not set
# CONFIG_STM32_PERFORMANCELINE is not set
# CONFIG_STM32_HIGHDENSITY is not set
# CONFIG_STM32_MEDIUMDENSITY is not set
# CONFIG_STM32_LOWDENSITY is not set
# CONFIG_STM32_STM32F20XX is not set
# CONFIG_STM32_STM32F30XX is not set
CONFIG_STM32_STM32F40XX=y
CONFIG_STM32_STM32F427=y
# CONFIG_STM32_DFU is not set
#
# STM32 Peripheral Support
#
CONFIG_STM32_ADC1=y
# CONFIG_STM32_ADC2 is not set
# CONFIG_STM32_ADC3 is not set
CONFIG_STM32_BKPSRAM=y
# CONFIG_STM32_CAN1 is not set
# CONFIG_STM32_CAN2 is not set
CONFIG_STM32_CCMDATARAM=y
# CONFIG_STM32_CRC is not set
# CONFIG_STM32_CRYP is not set
CONFIG_STM32_DMA1=y
CONFIG_STM32_DMA2=y
# CONFIG_STM32_DAC1 is not set
# CONFIG_STM32_DAC2 is not set
# CONFIG_STM32_DCMI is not set
# CONFIG_STM32_ETHMAC is not set
# CONFIG_STM32_FSMC is not set
# CONFIG_STM32_HASH is not set
# CONFIG_STM32_I2C1 is not set
CONFIG_STM32_I2C2=y
# CONFIG_STM32_I2C3 is not set
# CONFIG_STM32_OTGFS is not set
# CONFIG_STM32_OTGHS is not set
CONFIG_STM32_PWR=y
# CONFIG_STM32_RNG is not set
# CONFIG_STM32_SDIO is not set
CONFIG_STM32_SPI1=y
CONFIG_STM32_SPI2=y
CONFIG_STM32_SPI3=y
CONFIG_STM32_SPI4=y
# CONFIG_STM32_SPI5 is not set
# CONFIG_STM32_SPI6 is not set
CONFIG_STM32_SYSCFG=y
CONFIG_STM32_TIM1=y
# CONFIG_STM32_TIM2 is not set
CONFIG_STM32_TIM3=y
CONFIG_STM32_TIM4=y
CONFIG_STM32_TIM5=y
# CONFIG_STM32_TIM6 is not set
# CONFIG_STM32_TIM7 is not set
# CONFIG_STM32_TIM8 is not set
CONFIG_STM32_TIM9=y
# CONFIG_STM32_TIM10 is not set
# CONFIG_STM32_TIM11 is not set
# CONFIG_STM32_TIM12 is not set
# CONFIG_STM32_TIM13 is not set
# CONFIG_STM32_TIM14 is not set
CONFIG_STM32_USART1=y
CONFIG_STM32_USART2=y
CONFIG_STM32_USART3=y
# CONFIG_STM32_UART4 is not set
# CONFIG_STM32_UART5 is not set
# CONFIG_STM32_USART6 is not set
CONFIG_STM32_UART7=y
CONFIG_STM32_UART8=y
# CONFIG_STM32_IWDG is not set
CONFIG_STM32_WWDG=y
CONFIG_STM32_ADC=y
CONFIG_STM32_SPI=y
CONFIG_STM32_I2C=y
#
# Alternate Pin Mapping
#
CONFIG_STM32_FLASH_PREFETCH=y
# CONFIG_STM32_JTAG_DISABLE is not set
# CONFIG_STM32_JTAG_FULL_ENABLE is not set
# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
CONFIG_STM32_JTAG_SW_ENABLE=y
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
# CONFIG_STM32_FORCEPOWER is not set
# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
# CONFIG_STM32_CCMEXCLUDE is not set
CONFIG_STM32_DMACAPABLE=y
# CONFIG_STM32_TIM1_PWM is not set
# CONFIG_STM32_TIM3_PWM is not set
# CONFIG_STM32_TIM4_PWM is not set
# CONFIG_STM32_TIM5_PWM is not set
# CONFIG_STM32_TIM9_PWM is not set
# CONFIG_STM32_TIM1_ADC is not set
# CONFIG_STM32_TIM3_ADC is not set
# CONFIG_STM32_TIM4_ADC is not set
# CONFIG_STM32_TIM5_ADC is not set
CONFIG_STM32_USART=y
#
# U[S]ART Configuration
#
# CONFIG_USART1_RS485 is not set
CONFIG_USART1_RXDMA=y
# CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RS485 is not set
CONFIG_USART3_RXDMA=y
# CONFIG_UART7_RS485 is not set
CONFIG_UART7_RXDMA=y
# CONFIG_UART8_RS485 is not set
CONFIG_UART8_RXDMA=y
CONFIG_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_USART_SINGLEWIRE=y
#
# SPI Configuration
#
# CONFIG_STM32_SPI_INTERRUPTS is not set
# CONFIG_STM32_SPI_DMA is not set
#
# I2C Configuration
#
# CONFIG_STM32_I2C_DYNTIMEO is not set
CONFIG_STM32_I2CTIMEOSEC=0
CONFIG_STM32_I2CTIMEOMS=10
CONFIG_STM32_I2CTIMEOTICKS=500
# CONFIG_STM32_I2C_DUTY16_9 is not set
#
# USB Host Configuration
#
#
# USB Device Configuration
#
#
# External Memory Configuration
#
#
# Architecture Options
#
# CONFIG_ARCH_NOINTC is not set
# CONFIG_ARCH_VECNOTIRQ is not set
CONFIG_ARCH_DMA=y
CONFIG_ARCH_IRQPRIO=y
# CONFIG_CUSTOM_STACK is not set
# CONFIG_ADDRENV is not set
CONFIG_ARCH_HAVE_VFORK=y
CONFIG_ARCH_STACKDUMP=y
# CONFIG_ENDIAN_BIG is not set
# CONFIG_ARCH_HAVE_RAMFUNCS is not set
CONFIG_ARCH_HAVE_RAMVECTORS=y
# CONFIG_ARCH_RAMVECTORS is not set
#
# Board Settings
#
CONFIG_BOARD_LOOPSPERMSEC=16717
# CONFIG_ARCH_CALIBRATION is not set
CONFIG_DRAM_START=0x20000000
CONFIG_DRAM_SIZE=262144
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
CONFIG_ARCH_INTERRUPTSTACK=4096
#
# Boot options
#
# CONFIG_BOOT_RUNFROMEXTSRAM is not set
CONFIG_BOOT_RUNFROMFLASH=y
# CONFIG_BOOT_RUNFROMISRAM is not set
# CONFIG_BOOT_RUNFROMSDRAM is not set
# CONFIG_BOOT_COPYTORAM is not set
#
# Board Selection
#
CONFIG_ARCH_BOARD_CUSTOM=y
CONFIG_ARCH_BOARD=""
#
# Common Board Options
#
CONFIG_NSH_MMCSDMINOR=0
#
# Board-Specific Options
#
#
# RTOS Features
#
# CONFIG_BOARD_INITIALIZE is not set
CONFIG_MSEC_PER_TICK=1
CONFIG_RR_INTERVAL=0
CONFIG_SCHED_INSTRUMENTATION=y
CONFIG_TASK_NAME_SIZE=24
# CONFIG_SCHED_HAVE_PARENT is not set
# CONFIG_JULIAN_TIME is not set
CONFIG_START_YEAR=1970
CONFIG_START_MONTH=1
CONFIG_START_DAY=1
CONFIG_DEV_CONSOLE=y
# CONFIG_MUTEX_TYPES is not set
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_SEM_PREALLOCHOLDERS=0
CONFIG_SEM_NNESTPRIO=8
# CONFIG_FDCLONE_DISABLE is not set
CONFIG_FDCLONE_STDIO=y
CONFIG_SDCLONE_DISABLE=y
CONFIG_SCHED_WAITPID=y
# CONFIG_SCHED_STARTHOOK is not set
CONFIG_SCHED_ATEXIT=y
CONFIG_SCHED_ATEXIT_MAX=1
# CONFIG_SCHED_ONEXIT is not set
CONFIG_USER_ENTRYPOINT="nsh_main"
# CONFIG_DISABLE_OS_API is not set
#
# Signal Numbers
#
CONFIG_SIG_SIGUSR1=1
CONFIG_SIG_SIGUSR2=2
CONFIG_SIG_SIGALARM=3
CONFIG_SIG_SIGCONDTIMEDOUT=16
CONFIG_SIG_SIGWORK=4
#
# Sizes of configurable things (0 disables)
#
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=36
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
CONFIG_MQ_MAXMSGSIZE=32
CONFIG_MAX_WDOGPARMS=2
CONFIG_PREALLOC_WDOGS=50
CONFIG_PREALLOC_TIMERS=50
#
# Stack and heap information
#
CONFIG_IDLETHREAD_STACKSIZE=6000
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
#
# Device Drivers
#
# CONFIG_DISABLE_POLL is not set
CONFIG_DEV_NULL=y
# CONFIG_DEV_ZERO is not set
# CONFIG_LOOP is not set
# CONFIG_RAMDISK is not set
# CONFIG_CAN is not set
# CONFIG_PWM is not set
CONFIG_I2C=y
# CONFIG_I2C_SLAVE is not set
CONFIG_I2C_TRANSFER=y
# CONFIG_I2C_WRITEREAD is not set
# CONFIG_I2C_POLLED is not set
# CONFIG_I2C_TRACE is not set
CONFIG_ARCH_HAVE_I2CRESET=y
CONFIG_I2C_RESET=y
CONFIG_SPI=y
# CONFIG_SPI_OWNBUS is not set
CONFIG_SPI_EXCHANGE=y
# CONFIG_SPI_CMDDATA is not set
# CONFIG_RTC is not set
CONFIG_WATCHDOG=y
# CONFIG_ANALOG is not set
# CONFIG_AUDIO_DEVICES is not set
# CONFIG_BCH is not set
# CONFIG_INPUT is not set
# CONFIG_LCD is not set
# CONFIG_MMCSD is not set
CONFIG_MTD=y
#
# MTD Configuration
#
CONFIG_MTD_PARTITION=y
CONFIG_MTD_BYTE_WRITE=y
#
# MTD Device Drivers
#
# CONFIG_RAMMTD is not set
# CONFIG_MTD_AT24XX is not set
# CONFIG_MTD_AT45DB is not set
# CONFIG_MTD_M25P is not set
# CONFIG_MTD_SMART is not set
CONFIG_MTD_RAMTRON=y
CONFIG_RAMTRON_FUJITSU=y
# CONFIG_MTD_SST25 is not set
# CONFIG_MTD_SST39FV is not set
# CONFIG_MTD_W25 is not set
CONFIG_PIPES=y
# CONFIG_PM is not set
# CONFIG_POWER is not set
# CONFIG_SENSORS is not set
CONFIG_SERIAL=y
# CONFIG_DEV_LOWCONSOLE is not set
# CONFIG_16550_UART is not set
CONFIG_ARCH_HAVE_UART7=y
CONFIG_ARCH_HAVE_UART8=y
CONFIG_ARCH_HAVE_USART1=y
CONFIG_ARCH_HAVE_USART2=y
CONFIG_ARCH_HAVE_USART3=y
CONFIG_MCU_SERIAL=y
CONFIG_STANDARD_SERIAL=y
CONFIG_SERIAL_NPOLLWAITERS=2
# CONFIG_USART1_SERIAL_CONSOLE is not set
# CONFIG_USART2_SERIAL_CONSOLE is not set
# CONFIG_USART3_SERIAL_CONSOLE is not set
# CONFIG_UART7_SERIAL_CONSOLE is not set
CONFIG_UART8_SERIAL_CONSOLE=y
# CONFIG_NO_SERIAL_CONSOLE is not set
#
# USART1 Configuration
#
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART1_BAUD=115200
CONFIG_USART1_BITS=8
CONFIG_USART1_PARITY=0
CONFIG_USART1_2STOP=0
# CONFIG_USART1_IFLOWCONTROL is not set
# CONFIG_USART1_OFLOWCONTROL is not set
#
# USART2 Configuration
#
CONFIG_USART2_RXBUFSIZE=512
CONFIG_USART2_TXBUFSIZE=512
CONFIG_USART2_BAUD=115200
CONFIG_USART2_BITS=8
CONFIG_USART2_PARITY=0
CONFIG_USART2_2STOP=0
# CONFIG_USART2_IFLOWCONTROL is not set
# CONFIG_USART2_OFLOWCONTROL is not set
#
# USART3 Configuration
#
CONFIG_USART3_RXBUFSIZE=512
CONFIG_USART3_TXBUFSIZE=512
CONFIG_USART3_BAUD=115200
CONFIG_USART3_BITS=8
CONFIG_USART3_PARITY=0
CONFIG_USART3_2STOP=0
# CONFIG_USART3_IFLOWCONTROL is not set
# CONFIG_USART3_OFLOWCONTROL is not set
#
# UART7 Configuration
#
CONFIG_UART7_RXBUFSIZE=512
CONFIG_UART7_TXBUFSIZE=512
CONFIG_UART7_BAUD=115200
CONFIG_UART7_BITS=8
CONFIG_UART7_PARITY=0
CONFIG_UART7_2STOP=0
# CONFIG_UART7_IFLOWCONTROL is not set
# CONFIG_UART7_OFLOWCONTROL is not set
#
# UART8 Configuration
#
CONFIG_UART8_RXBUFSIZE=512
CONFIG_UART8_TXBUFSIZE=512
CONFIG_UART8_BAUD=115200
CONFIG_UART8_BITS=8
CONFIG_UART8_PARITY=0
CONFIG_UART8_2STOP=0
# CONFIG_UART8_IFLOWCONTROL is not set
# CONFIG_UART8_OFLOWCONTROL is not set
# CONFIG_SERIAL_IFLOWCONTROL is not set
# CONFIG_SERIAL_OFLOWCONTROL is not set
# CONFIG_USBDEV is not set
# CONFIG_USBHOST is not set
# CONFIG_WIRELESS is not set
#
# System Logging Device Options
#
#
# System Logging
#
# CONFIG_RAMLOG is not set
#
# Networking Support
#
# CONFIG_NET is not set
#
# File Systems
#
#
# File system configuration
#
# CONFIG_DISABLE_MOUNTPOINT is not set
# CONFIG_FS_RAMMAP is not set
CONFIG_FS_FAT=y
CONFIG_FAT_LCNAMES=y
CONFIG_FAT_LFN=y
CONFIG_FAT_MAXFNAME=32
CONFIG_FS_FATTIME=y
CONFIG_FAT_DMAMEMORY=y
CONFIG_FS_NXFFS=y
CONFIG_NXFFS_PREALLOCATED=y
CONFIG_NXFFS_ERASEDSTATE=0xff
CONFIG_NXFFS_PACKTHRESHOLD=32
CONFIG_NXFFS_MAXNAMLEN=32
CONFIG_NXFFS_TAILTHRESHOLD=2048
CONFIG_FS_ROMFS=y
# CONFIG_FS_SMARTFS is not set
CONFIG_FS_BINFS=y
#
# System Logging
#
# CONFIG_SYSLOG_ENABLE is not set
# CONFIG_SYSLOG is not set
#
# Graphics Support
#
# CONFIG_NX is not set
#
# Memory Management
#
# CONFIG_MM_MULTIHEAP is not set
# CONFIG_MM_SMALL is not set
CONFIG_MM_REGIONS=2
CONFIG_GRAN=y
# CONFIG_GRAN_SINGLE is not set
# CONFIG_GRAN_INTR is not set
#
# Audio Support
#
# CONFIG_AUDIO is not set
#
# Binary Formats
#
# CONFIG_BINFMT_DISABLE is not set
# CONFIG_BINFMT_EXEPATH is not set
# CONFIG_NXFLAT is not set
# CONFIG_ELF is not set
CONFIG_BUILTIN=y
# CONFIG_PIC is not set
# CONFIG_SYMTAB_ORDEREDBYNAME is not set
#
# Library Routines
#
#
# Standard C Library Options
#
CONFIG_STDIO_BUFFER_SIZE=32
CONFIG_STDIO_LINEBUFFER=y
CONFIG_NUNGET_CHARS=2
CONFIG_LIB_HOMEDIR="/"
# CONFIG_NOPRINTF_FIELDWIDTH is not set
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_LIB_RAND_ORDER=1
# CONFIG_EOL_IS_CR is not set
# CONFIG_EOL_IS_LF is not set
# CONFIG_EOL_IS_BOTH_CRLF is not set
CONFIG_EOL_IS_EITHER_CRLF=y
# CONFIG_LIBC_EXECFUNCS is not set
CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
CONFIG_LIBC_STRERROR=y
# CONFIG_LIBC_STRERROR_SHORT is not set
# CONFIG_LIBC_PERROR_STDOUT is not set
CONFIG_ARCH_LOWPUTC=y
CONFIG_LIB_SENDFILE_BUFSIZE=512
# CONFIG_ARCH_ROMGETC is not set
CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
CONFIG_ARCH_MEMCPY=y
# CONFIG_ARCH_MEMCMP is not set
# CONFIG_ARCH_MEMMOVE is not set
# CONFIG_ARCH_MEMSET is not set
# CONFIG_MEMSET_OPTSPEED is not set
# CONFIG_ARCH_STRCHR is not set
# CONFIG_ARCH_STRCMP is not set
# CONFIG_ARCH_STRCPY is not set
# CONFIG_ARCH_STRNCPY is not set
# CONFIG_ARCH_STRLEN is not set
# CONFIG_ARCH_STRNLEN is not set
# CONFIG_ARCH_BZERO is not set
#
# Non-standard Library Support
#
CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_WORKPRIORITY=192
CONFIG_SCHED_WORKPERIOD=5000
CONFIG_SCHED_WORKSTACKSIZE=2000
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKPERIOD=50000
CONFIG_SCHED_LPWORKSTACKSIZE=2000
# CONFIG_LIB_KBDCODEC is not set
# CONFIG_LIB_SLCDCODEC is not set
#
# Basic CXX Support
#
CONFIG_C99_BOOL8=y
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
# CONFIG_CXX_NEWLONG is not set
#
# uClibc++ Standard C++ Library
#
# CONFIG_UCLIBCXX is not set
#
# Application Configuration
#
#
# Built-In Applications
#
CONFIG_BUILTIN_PROXY_STACKSIZE=1024
#
# Examples
#
# CONFIG_EXAMPLES_BUTTONS is not set
# CONFIG_EXAMPLES_CAN is not set
# CONFIG_EXAMPLES_COMPOSITE is not set
# CONFIG_EXAMPLES_CXXTEST is not set
# CONFIG_EXAMPLES_DHCPD is not set
# CONFIG_EXAMPLES_ELF is not set
# CONFIG_EXAMPLES_FTPC is not set
# CONFIG_EXAMPLES_FTPD is not set
CONFIG_EXAMPLES_HELLO=y
# CONFIG_EXAMPLES_HELLOXX is not set
# CONFIG_EXAMPLES_JSON is not set
# CONFIG_EXAMPLES_HIDKBD is not set
# CONFIG_EXAMPLES_KEYPADTEST is not set
# CONFIG_EXAMPLES_IGMP is not set
# CONFIG_EXAMPLES_LCDRW is not set
# CONFIG_EXAMPLES_MM is not set
# CONFIG_EXAMPLES_MODBUS is not set
CONFIG_EXAMPLES_MOUNT=y
# CONFIG_EXAMPLES_MTDPART is not set
# CONFIG_EXAMPLES_NRF24L01TERM is not set
CONFIG_EXAMPLES_NSH=y
# CONFIG_EXAMPLES_NULL is not set
# CONFIG_EXAMPLES_NX is not set
# CONFIG_EXAMPLES_NXCONSOLE is not set
# CONFIG_EXAMPLES_NXFFS is not set
# CONFIG_EXAMPLES_NXFLAT is not set
# CONFIG_EXAMPLES_NXHELLO is not set
# CONFIG_EXAMPLES_NXIMAGE is not set
# CONFIG_EXAMPLES_NXLINES is not set
# CONFIG_EXAMPLES_NXTEXT is not set
# CONFIG_EXAMPLES_OSTEST is not set
# CONFIG_EXAMPLES_PASHELLO is not set
# CONFIG_EXAMPLES_PIPE is not set
# CONFIG_EXAMPLES_POSIXSPAWN is not set
# CONFIG_EXAMPLES_QENCODER is not set
# CONFIG_EXAMPLES_RGMP is not set
# CONFIG_EXAMPLES_ROMFS is not set
# CONFIG_EXAMPLES_SENDMAIL is not set
# CONFIG_EXAMPLES_SERLOOP is not set
# CONFIG_EXAMPLES_SLCD is not set
# CONFIG_EXAMPLES_SMART_TEST is not set
# CONFIG_EXAMPLES_SMART is not set
# CONFIG_EXAMPLES_TCPECHO is not set
# CONFIG_EXAMPLES_TELNETD is not set
# CONFIG_EXAMPLES_THTTPD is not set
# CONFIG_EXAMPLES_TIFF is not set
# CONFIG_EXAMPLES_TOUCHSCREEN is not set
# CONFIG_EXAMPLES_UDP is not set
# CONFIG_EXAMPLES_UIP is not set
# CONFIG_EXAMPLES_USBSERIAL is not set
# CONFIG_EXAMPLES_USBMSC is not set
# CONFIG_EXAMPLES_USBTERM is not set
# CONFIG_EXAMPLES_WATCHDOG is not set
#
# Graphics Support
#
# CONFIG_TIFF is not set
#
# Interpreters
#
# CONFIG_INTERPRETERS_FICL is not set
# CONFIG_INTERPRETERS_PCODE is not set
#
# Network Utilities
#
#
# Networking Utilities
#
# CONFIG_NETUTILS_CODECS is not set
# CONFIG_NETUTILS_DHCPC is not set
# CONFIG_NETUTILS_DHCPD is not set
# CONFIG_NETUTILS_FTPC is not set
# CONFIG_NETUTILS_FTPD is not set
# CONFIG_NETUTILS_JSON is not set
# CONFIG_NETUTILS_RESOLV is not set
# CONFIG_NETUTILS_SMTP is not set
# CONFIG_NETUTILS_TELNETD is not set
# CONFIG_NETUTILS_TFTPC is not set
# CONFIG_NETUTILS_THTTPD is not set
# CONFIG_NETUTILS_UIPLIB is not set
# CONFIG_NETUTILS_WEBCLIENT is not set
#
# FreeModBus
#
# CONFIG_MODBUS is not set
#
# NSH Library
#
CONFIG_NSH_LIBRARY=y
CONFIG_NSH_BUILTIN_APPS=y
#
# Disable Individual commands
#
# CONFIG_NSH_DISABLE_CAT is not set
# CONFIG_NSH_DISABLE_CD is not set
# CONFIG_NSH_DISABLE_CP is not set
# CONFIG_NSH_DISABLE_DD is not set
# CONFIG_NSH_DISABLE_ECHO is not set
# CONFIG_NSH_DISABLE_EXEC is not set
# CONFIG_NSH_DISABLE_EXIT is not set
# CONFIG_NSH_DISABLE_FREE is not set
# CONFIG_NSH_DISABLE_GET is not set
# CONFIG_NSH_DISABLE_HELP is not set
# CONFIG_NSH_DISABLE_HEXDUMP is not set
# CONFIG_NSH_DISABLE_IFCONFIG is not set
# CONFIG_NSH_DISABLE_KILL is not set
# CONFIG_NSH_DISABLE_LOSETUP is not set
# CONFIG_NSH_DISABLE_LS is not set
# CONFIG_NSH_DISABLE_MB is not set
# CONFIG_NSH_DISABLE_MKDIR is not set
# CONFIG_NSH_DISABLE_MKFATFS is not set
# CONFIG_NSH_DISABLE_MKFIFO is not set
# CONFIG_NSH_DISABLE_MKRD is not set
# CONFIG_NSH_DISABLE_MH is not set
# CONFIG_NSH_DISABLE_MOUNT is not set
# CONFIG_NSH_DISABLE_MW is not set
# CONFIG_NSH_DISABLE_NSFMOUNT is not set
# CONFIG_NSH_DISABLE_PS is not set
# CONFIG_NSH_DISABLE_PING is not set
# CONFIG_NSH_DISABLE_PUT is not set
# CONFIG_NSH_DISABLE_PWD is not set
# CONFIG_NSH_DISABLE_RM is not set
# CONFIG_NSH_DISABLE_RMDIR is not set
# CONFIG_NSH_DISABLE_SET is not set
# CONFIG_NSH_DISABLE_SH is not set
# CONFIG_NSH_DISABLE_SLEEP is not set
# CONFIG_NSH_DISABLE_TEST is not set
# CONFIG_NSH_DISABLE_UMOUNT is not set
# CONFIG_NSH_DISABLE_UNSET is not set
# CONFIG_NSH_DISABLE_USLEEP is not set
# CONFIG_NSH_DISABLE_WGET is not set
# CONFIG_NSH_DISABLE_XD is not set
#
# Configure Command Options
#
# CONFIG_NSH_CMDOPT_DF_H is not set
CONFIG_NSH_CODECS_BUFSIZE=128
CONFIG_NSH_FILEIOSIZE=512
CONFIG_NSH_STRERROR=y
CONFIG_NSH_LINELEN=128
CONFIG_NSH_MAXARGUMENTS=12
CONFIG_NSH_NESTDEPTH=8
# CONFIG_NSH_DISABLESCRIPT is not set
# CONFIG_NSH_DISABLEBG is not set
CONFIG_NSH_ROMFSETC=y
# CONFIG_NSH_ROMFSRC is not set
CONFIG_NSH_ROMFSMOUNTPT="/etc"
CONFIG_NSH_INITSCRIPT="init.d/rcS"
CONFIG_NSH_ROMFSDEVNO=0
CONFIG_NSH_ROMFSSECTSIZE=128
CONFIG_NSH_ARCHROMFS=y
CONFIG_NSH_FATDEVNO=1
CONFIG_NSH_FATSECTSIZE=512
CONFIG_NSH_FATNSECTORS=1024
CONFIG_NSH_FATMOUNTPT="/tmp"
CONFIG_NSH_CONSOLE=y
#
# USB Trace Support
#
# CONFIG_NSH_CONDEV is not set
CONFIG_NSH_ARCHINIT=y
#
# NxWidgets/NxWM
#
#
# System NSH Add-Ons
#
#
# Custom Free Memory Command
#
# CONFIG_SYSTEM_FREE is not set
#
# I2C tool
#
CONFIG_SYSTEM_I2CTOOL=y
CONFIG_I2CTOOL_MINBUS=0
CONFIG_I2CTOOL_MAXBUS=3
CONFIG_I2CTOOL_MINADDR=0x03
CONFIG_I2CTOOL_MAXADDR=0x77
CONFIG_I2CTOOL_MAXREGADDR=0xff
CONFIG_I2CTOOL_DEFFREQ=4000000
#
# FLASH Program Installation
#
# CONFIG_SYSTEM_INSTALL is not set
#
# FLASH Erase-all Command
#
# CONFIG_SYSTEM_FLASH_ERASEALL is not set
#
# readline()
#
CONFIG_SYSTEM_READLINE=y
CONFIG_READLINE_ECHO=y
#
# Power Off
#
# CONFIG_SYSTEM_POWEROFF is not set
#
# RAMTRON
#
# CONFIG_SYSTEM_RAMTRON is not set
#
# SD Card
#
# CONFIG_SYSTEM_SDCARD is not set
#
# Sysinfo
#
CONFIG_SYSTEM_SYSINFO=y
#
# USB Monitor
#

67
nuttx-configs/aerocore/nsh/setenv.sh Executable file
View File

@ -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}"

150
nuttx-configs/aerocore/scripts/ld.script Normal file
View File

@ -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) }
}

84
nuttx-configs/aerocore/src/Makefile Normal file
View File

@ -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

4
nuttx-configs/aerocore/src/empty.c Normal file
View File

@ -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.
*/

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -418,7 +418,7 @@ CONFIG_PREALLOC_TIMERS=50
# Stack and heap information # Stack and heap information
# #
CONFIG_IDLETHREAD_STACKSIZE=4096 CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_USERMAIN_STACKSIZE=4096 CONFIG_USERMAIN_STACKSIZE=3500
CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048 CONFIG_PTHREAD_STACK_DEFAULT=2048

282
src/drivers/boards/aerocore/aerocore_init.c Normal file
View File

@ -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;
}

142
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c → src/drivers/boards/aerocore/aerocore_led.c
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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 * AeroCore LED backend.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/ */
#include <nuttx/config.h> #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
/** __EXPORT void led_init()
* Velocity estimate delay {
* stm32_configgpio(GPIO_LED0);
* The delay in milliseconds of the velocity estimate from GPS. stm32_configgpio(GPIO_LED1);
* }
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/** __EXPORT void led_on(int led)
* Position estimate delay {
* switch (led) {
* The delay in milliseconds of the position estimate from GPS. case 0:
* stm32_gpiowrite(GPIO_LED0, true);
* @min 0 break;
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/** case 1:
* Height estimate delay stm32_gpiowrite(GPIO_LED1, true);
* break;
* 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);
/** default:
* Mag estimate delay warnx("LED ID not recognized\n");
* }
* 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);
/** __EXPORT void led_off(int led)
* True airspeeed estimate delay {
* switch (led) {
* The delay in milliseconds of the airspeed estimate. case 0:
* stm32_gpiowrite(GPIO_LED0, false);
* @min 0 break;
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/** case 1:
* GPS vs. barometric altitude update weight stm32_gpiowrite(GPIO_LED1, false);
* break;
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
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");
}
}

117
src/drivers/boards/aerocore/aerocore_pwm_servo.c Normal file
View File

@ -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,
}
};

183
src/drivers/boards/aerocore/aerocore_spi.c Normal file
View File

@ -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

176
src/drivers/boards/aerocore/board_config.h Normal file
View File

@ -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

8
src/drivers/boards/aerocore/module.mk Normal file
View File

@ -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

2
src/drivers/boards/px4fmu-v1/board_config.h
View File

@ -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_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 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 * Use these in place of the spi_dev_e enumeration to
* select a specific SPI device on SPI1 * select a specific SPI device on SPI1

2
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -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_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 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 */ /* 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_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_ACCEL_MAG 2

10
src/drivers/drv_gpio.h
View File

@ -94,6 +94,14 @@
#endif #endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
/*
* AeroCore GPIO numbers and configuration.
*
*/
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
/* no GPIO driver on the PX4IOv1 board */ /* no GPIO driver on the PX4IOv1 board */
#endif #endif
@ -146,4 +154,4 @@
#define GPIO_SENSOR_RAIL_RESET GPIOC(13) #define GPIO_SENSOR_RAIL_RESET GPIOC(13)
#endif /* _DRV_GPIO_H */ #endif /* _DRV_GPIO_H */

4
src/drivers/drv_gps.h
View File

@ -43,10 +43,14 @@
#include <stdint.h> #include <stdint.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include "board_config.h"
#include "drv_sensor.h" #include "drv_sensor.h"
#include "drv_orb_dev.h" #include "drv_orb_dev.h"
#ifndef GPS_DEFAULT_UART_PORT
#define GPS_DEFAULT_UART_PORT "/dev/ttyS3" #define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
#endif
#define GPS_DEVICE_PATH "/dev/gps" #define GPS_DEVICE_PATH "/dev/gps"

2
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -53,6 +53,8 @@
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
/* FrSky sensor hub data IDs */ /* FrSky sensor hub data IDs */
#define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_GPS_ALT_BP 0x01
#define FRSKY_ID_TEMP1 0x02 #define FRSKY_ID_TEMP1 0x02

2
src/drivers/frsky_telemetry/frsky_telemetry.c
View File

@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[])
frsky_task = task_spawn_cmd("frsky_telemetry", frsky_task = task_spawn_cmd("frsky_telemetry",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 2000,
frsky_telemetry_thread_main, frsky_telemetry_thread_main,
(const char **)argv); (const char **)argv);

2
src/drivers/frsky_telemetry/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.c \ SRCS = frsky_data.c \
frsky_telemetry.c frsky_telemetry.c
MODULE_STACKSIZE = 1200

29
src/drivers/gps/gps.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -56,6 +56,7 @@
#include <arch/board/board.h> #include <arch/board/board.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h> #include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h> #include <systemlib/err.h>
@ -63,6 +64,8 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <board_config.h>
#include "ubx.h" #include "ubx.h"
#include "mtk.h" #include "mtk.h"
@ -76,12 +79,6 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class GPS : public device::CDev class GPS : public device::CDev
{ {
public: public:
@ -209,7 +206,8 @@ GPS::init()
goto out; goto out;
/* start the GPS driver worker task */ /* start the GPS driver worker task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); _task = task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) { if (_task < 0) {
warnx("task start failed: %d", errno); warnx("task start failed: %d", errno);
@ -276,14 +274,14 @@ GPS::task_main()
_report.timestamp_position = hrt_absolute_time(); _report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f; _report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f; _report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)400e3f; _report.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time(); _report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f; _report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f; _report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f; _report.c_variance_rad = 0.1f;
_report.fix_type = 3; _report.fix_type = 3;
_report.eph_m = 3.0f; _report.eph_m = 0.9f;
_report.epv_m = 7.0f; _report.epv_m = 1.8f;
_report.timestamp_velocity = hrt_absolute_time(); _report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f; _report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f; _report.vel_e_m_s = 0.0f;
@ -421,7 +419,14 @@ GPS::task_main()
void void
GPS::cmd_reset() 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 void

2
src/drivers/gps/module.mk
View File

@ -41,3 +41,5 @@ SRCS = gps.cpp \
gps_helper.cpp \ gps_helper.cpp \
mtk.cpp \ mtk.cpp \
ubx.cpp ubx.cpp
MODULE_STACKSIZE = 1200

56
src/drivers/l3gd20/l3gd20.cpp
View File

@ -34,6 +34,9 @@
/** /**
* @file l3gd20.cpp * @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI. * 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> #include <nuttx/config.h>
@ -89,9 +92,11 @@ static const int ERROR = -1;
#define ADDR_WHO_AM_I 0x0F #define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM_H 0xD7 #define WHO_I_AM_H 0xD7
#define WHO_I_AM 0xD4 #define WHO_I_AM 0xD4
#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
#define ADDR_CTRL_REG1 0x20 #define ADDR_CTRL_REG1 0x20
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
/* keep lowpass low to avoid noise issues */ /* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #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)) #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 FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
#define L3GD20_DEFAULT_RATE 760 #define L3GD20_DEFAULT_RATE 760
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30 #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[]); } extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI class L3GD20 : public device::SPI
@ -216,6 +226,9 @@ private:
math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z; math::LowPassFilter2p _gyro_filter_z;
/* true if an L3G4200D is detected */
bool _is_l3g4200d;
/** /**
* Start automatic measurement. * Start automatic measurement.
*/ */
@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_topic(-1), _gyro_topic(-1),
_class_instance(-1), _class_instance(-1),
_current_rate(0), _current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG), _orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0), _read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(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 // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -413,14 +427,7 @@ L3GD20::probe()
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 _orientation = SENSOR_BOARD_ROTATION_DEFAULT;
_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
success = true; success = true;
} }
@ -430,6 +437,13 @@ L3GD20::probe()
success = true; 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) if (success)
return OK; return OK;
@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
if (_is_l3g4200d) {
return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
}
return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */ /* 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; uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
if (frequency == 0) 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) { if (frequency <= 100) {
_current_rate = 95; _current_rate = _is_l3g4200d ? 100 : 95;
bits |= RATE_95HZ_LP_25HZ; bits |= RATE_95HZ_LP_25HZ;
} else if (frequency <= 200) { } else if (frequency <= 200) {
_current_rate = 190; _current_rate = _is_l3g4200d ? 200 : 190;
bits |= RATE_190HZ_LP_50HZ; bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) { } else if (frequency <= 400) {
_current_rate = 380; _current_rate = _is_l3g4200d ? 400 : 380;
bits |= RATE_380HZ_LP_50HZ; bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) { } else if (frequency <= 800) {
_current_rate = 760; _current_rate = _is_l3g4200d ? 800 : 760;
bits |= RATE_760HZ_LP_50HZ; bits |= RATE_760HZ_LP_50HZ;
} else { } else {
return -EINVAL; return -EINVAL;
@ -772,7 +792,7 @@ L3GD20::reset()
* callback fast enough to not miss data. */ * callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); 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_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
@ -971,7 +991,7 @@ start()
errx(0, "already started"); errx(0, "already started");
/* create the driver */ /* 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) if (g_dev == nullptr)
goto fail; goto fail;

2
src/drivers/lsm303d/lsm303d.cpp
View File

@ -1793,7 +1793,7 @@ start()
errx(0, "already started"); errx(0, "already started");
/* create the driver */ /* 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) { if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj"); warnx("failed instantiating LSM303D obj");

2
src/drivers/ms5611/ms5611_spi.cpp
View File

@ -117,7 +117,7 @@ private:
device::Device * device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf) 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) : MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :

83
src/drivers/px4fmu/fmu.cpp
View File

@ -92,6 +92,7 @@ public:
MODE_2PWM, MODE_2PWM,
MODE_4PWM, MODE_4PWM,
MODE_6PWM, MODE_6PWM,
MODE_8PWM,
}; };
PX4FMU(); PX4FMU();
virtual ~PX4FMU(); virtual ~PX4FMU();
@ -113,6 +114,9 @@ private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6; static const unsigned _max_actuators = 6;
#endif #endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
static const unsigned _max_actuators = 8;
#endif
Mode _mode; Mode _mode;
unsigned _pwm_default_rate; unsigned _pwm_default_rate;
@ -203,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif #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]); const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@ -382,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
break; 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: case MODE_NONE:
debug("MODE_NONE"); debug("MODE_NONE");
@ -602,6 +634,9 @@ PX4FMU::task_main()
num_outputs = 6; num_outputs = 6;
break; break;
case MODE_8PWM:
num_outputs = 8;
break;
default: default:
num_outputs = 0; num_outputs = 0;
break; break;
@ -757,6 +792,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM: case MODE_2PWM:
case MODE_4PWM: case MODE_4PWM:
case MODE_6PWM: case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
#endif
ret = pwm_ioctl(filp, cmd, arg); ret = pwm_ioctl(filp, cmd, arg);
break; break;
@ -986,6 +1024,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break; 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(5):
case PWM_SERVO_SET(4): case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) { if (_mode < MODE_6PWM) {
@ -1013,6 +1060,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break; 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(5):
case PWM_SERVO_GET(4): case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) { 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(3):
case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5): 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)); *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break; break;
case PWM_SERVO_GET_COUNT: case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT: case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) { switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
case MODE_6PWM: case MODE_6PWM:
*(unsigned *)arg = 6; *(unsigned *)arg = 6;
break; break;
@ -1091,6 +1157,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_6PWM); set_mode(MODE_6PWM);
break; break;
#endif #endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
case 8:
set_mode(MODE_8PWM);
break;
#endif
default: default:
ret = -EINVAL; ret = -EINVAL;
@ -1181,10 +1252,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
unsigned count = len / 2; unsigned count = len / 2;
uint16_t values[6]; uint16_t values[6];
#ifdef CONFIG_ARCH_BOARD_AEROCORE
if (count > 8) {
// we have at most 8 outputs
count = 8;
}
#else
if (count > 6) { if (count > 6) {
// we have at most 6 outputs // we have at most 6 outputs
count = 6; count = 6;
} }
#endif
// allow for misaligned values // allow for misaligned values
memcpy(values, buffer, count * 2); memcpy(values, buffer, count * 2);
@ -1458,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
#endif #endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
servo_mode = PX4FMU::MODE_6PWM; servo_mode = PX4FMU::MODE_6PWM;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
servo_mode = PX4FMU::MODE_8PWM;
#endif #endif
break; break;
@ -1776,7 +1857,7 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #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"); 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"); fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif #endif
exit(1); exit(1);

2
src/drivers/px4fmu/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = fmu MODULE_COMMAND = fmu
SRCS = fmu.cpp SRCS = fmu.cpp
MODULE_STACKSIZE = 1200

2
src/drivers/px4io/module.mk
View File

@ -44,3 +44,5 @@ SRCS = px4io.cpp \
# XXX prune to just get UART registers # XXX prune to just get UART registers
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MODULE_STACKSIZE = 1200

55
src/drivers/px4io/px4io.cpp
View File

@ -529,6 +529,11 @@ PX4IO::~PX4IO()
if (_interface != nullptr) if (_interface != nullptr)
delete _interface; delete _interface;
/* deallocate perfs */
perf_free(_perf_update);
perf_free(_perf_write);
perf_free(_perf_chan_count);
g_dev = nullptr; g_dev = nullptr;
} }
@ -789,7 +794,12 @@ PX4IO::init()
} }
/* start the IO interface task */ /* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); _task = task_spawn_cmd("px4io",
SCHED_DEFAULT,
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
2000,
(main_t)&PX4IO::task_main_trampoline,
nullptr);
if (_task < 0) { if (_task < 0) {
debug("task start failed: %d", errno); debug("task start failed: %d", errno);
@ -984,13 +994,17 @@ PX4IO::task_main()
int32_t failsafe_param_val; int32_t failsafe_param_val;
param_t failsafe_param = param_find("RC_FAILS_THR"); param_t failsafe_param = param_find("RC_FAILS_THR");
if (failsafe_param > 0) { if (failsafe_param != PARAM_INVALID) {
param_get(failsafe_param, &failsafe_param_val); param_get(failsafe_param, &failsafe_param_val);
uint16_t failsafe_thr = failsafe_param_val;
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); if (failsafe_param_val > 0) {
if (pret != OK) {
log("failsafe upload failed"); uint16_t failsafe_thr = failsafe_param_val;
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
log("failsafe upload failed, FS: %d us", (int)failsafe_thr);
}
} }
} }
@ -1448,7 +1462,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 */ /* we don't have the status bits, so input_source has to be set elsewhere */
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; 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]; uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
/* /*
@ -1456,8 +1470,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). * 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, &regs[0], prolog + 9); ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
if (ret != OK) if (ret != OK)
@ -1469,23 +1481,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*/ */
channel_count = regs[PX4IO_P_RAW_RC_COUNT]; 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); perf_count(_perf_chan_count);
}
_rc_chan_count = channel_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.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; 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_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_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_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 */ /* 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; _rc_last_valid = input_rc.timestamp_publication;
}
input_rc.timestamp_last_signal = _rc_last_valid; 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) { if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9); ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@ -1493,8 +1520,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
return ret; return ret;
} }
input_rc.channel_count = channel_count; /* last thing set are the actual channel values as 16 bit values */
memcpy(input_rc.values, &regs[prolog], channel_count * 2); for (unsigned i = 0; i < channel_count; i++) {
input_rc.values[i] = regs[prolog + i];
}
return ret; return ret;
} }

4
src/drivers/stm32/adc/adc.cpp
View File

@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
#endif #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) if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver"); errx(1, "couldn't allocate the ADC driver");

4
src/drivers/stm32/drv_hrt.c
View File

@ -141,7 +141,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP # 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 # if CONFIG_STM32_TIM10
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
# endif # endif
@ -150,7 +150,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM # 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 # if CONFIG_STM32_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif # endif

2
src/examples/fixedwing_control/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = ex_fixedwing_control
SRCS = main.c \ SRCS = main.c \
params.c params.c
MODULE_STACKSIZE = 1200

5
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -65,6 +65,7 @@
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>
#include <uORB/topics/filtered_bottom_flow.h> #include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <poll.h> #include <poll.h>
#include "flow_position_estimator_params.h" #include "flow_position_estimator_params.h"
@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
daemon_task = task_spawn_cmd("flow_position_estimator", daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_RR, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
4096, 4000,
flow_position_estimator_thread_main, flow_position_estimator_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); exit(0);

2
src/examples/px4_daemon_app/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = px4_daemon_app MODULE_COMMAND = px4_daemon_app
SRCS = px4_daemon_app.c SRCS = px4_daemon_app.c
MODULE_STACKSIZE = 1200

2
src/examples/px4_daemon_app/px4_daemon_app.c
View File

@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("daemon", daemon_task = task_spawn_cmd("daemon",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
4096, 2000,
px4_daemon_thread_main, px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); exit(0);

4
src/examples/px4_mavlink_debug/module.mk
View File

@ -37,4 +37,6 @@
MODULE_COMMAND = px4_mavlink_debug MODULE_COMMAND = px4_mavlink_debug
SRCS = px4_mavlink_debug.c SRCS = px4_mavlink_debug.c
MODULE_STACKSIZE = 2000

4
src/lib/version/version.h
View File

@ -59,4 +59,8 @@
#define HW_ARCH "PX4FMU_V2" #define HW_ARCH "PX4FMU_V2"
#endif #endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
#define HW_ARCH "AEROCORE"
#endif
#endif /* VERSION_H_ */ #endif /* VERSION_H_ */

812
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -1,812 +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.cpp
*
* Kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rPressAlt(this, "R_PRESS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
memset(&ref, 0, sizeof(ref));
F.zero();
G.zero();
V.zero();
HAtt.zero();
RAtt.zero();
HPos.zero();
RPos.zero();
// initial state covariance matrix
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize rotation quaternion with a single raw sensor measurement
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
HPos(5, 8) = 1.0f;
// initialize all parameters
updateParams();
}
math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
return math::Quaternion(q0, q1, q2, q3);
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
warnx("initialized EKF attitude");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2
//&& _gps.counter_pos_valid > 10
) {
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
map_projection_init(&ref, lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
double(vN), double(vE), double(vD),
lat, lon, double(alt));
}
// prediction step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(&ref, lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = lat * M_RAD_TO_DEG;
_localPos.ref_lon = lon * M_RAD_TO_DEG;
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = q.to_dcm();
// euler update
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
// XXX position prediction using speed
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
// float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
// float sinPsi = sinf(psi);
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
if (yMag > M_PI_F) yMag -= 2*M_PI_F;
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(1, 1) = 1.0e10;
RAttAdjust(2, 2) = 1.0e10;
RAttAdjust(3, 3) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
// HAccel
HAtt(1, 1) = cosTheta;
HAtt(2, 0) = -cosPhi * cosTheta;
HAtt(2, 1) = sinPhi * sinTheta;
HAtt(3, 0) = sinPhi * cosTheta;
HAtt(3, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
warnx("numerical failure in att correction");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:"); y.print();
}
// update quaternions from euler
// angle correction
q.from_euler(phi, theta, psi);
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
// abort correction and return
warnx("numerical failure in gps correction");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(1, 1) = noiseAccelSq; // normalized direction
RAtt(2, 2) = noiseAccelSq;
RAtt(3, 3) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseGpsAlt = _rGpsAlt.get();
float noisePressAlt = _rPressAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
RPos(5, 5) = noisePressAlt * noisePressAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}

194
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
View File

@ -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; }
};

157
src/modules/att_pos_estimator_ekf/kalman_main.cpp
View File

@ -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;
}

11
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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. * Extended Kalman Filter for Attitude Estimation.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -112,7 +113,7 @@ usage(const char *reason)
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * 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[]) int attitude_estimator_ekf_main(int argc, char *argv[])
{ {

2
src/modules/attitude_estimator_ekf/module.mk
View File

@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
codegen/rtGetNaN.c \ codegen/rtGetNaN.c \
codegen/norm.c \ codegen/norm.c \
codegen/cross.c codegen/cross.c
MODULE_STACKSIZE = 1200

9
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -36,6 +34,9 @@
/* /*
* @file attitude_estimator_so3_main.cpp * @file attitude_estimator_so3_main.cpp
* *
* @author Hyon Lim <limhyon@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Implementation of nonlinear complementary filters on the SO(3). * Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@ -131,7 +132,7 @@ usage(const char *reason)
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_create(). * to task_spawn_cmd().
*/ */
int attitude_estimator_so3_main(int argc, char *argv[]) int attitude_estimator_so3_main(int argc, char *argv[])
{ {

2
src/modules/attitude_estimator_so3/module.mk
View File

@ -6,3 +6,5 @@ MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \ SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c attitude_estimator_so3_params.c
MODULE_STACKSIZE = 1200

5
src/modules/commander/commander.cpp
View File

@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander", daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40, SCHED_PRIORITY_MAX - 40,
3000, 2950,
commander_thread_main, commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
@ -743,7 +743,7 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_t commander_low_prio_attr; pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr); pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2992); pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
struct sched_param param; struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param); (void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@ -1710,6 +1710,7 @@ set_control_mode()
case MAIN_STATE_AUTO: case MAIN_STATE_AUTO:
navigator_enabled = true; navigator_enabled = true;
break;
default: default:
break; break;

2
src/modules/commander/commander_helper.cpp
View File

@ -206,7 +206,7 @@ int led_init()
return ERROR; return ERROR;
} }
/* the blue LED is only available on FMUv1 but not FMUv2 */ /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE); (void)ioctl(leds, LED_ON, LED_BLUE);
/* we consider the amber led mandatory */ /* we consider the amber led mandatory */

16
src/modules/commander/mag_calibration.cpp
View File

@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_interval = 45 * 1000 * 1000; uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */ /* maximum 500 values */
const unsigned int calibration_maxcount = 500; const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter; unsigned int calibration_counter;
struct mag_scale mscale_null = { struct mag_scale mscale_null = {
@ -121,6 +121,20 @@ int do_mag_calibration(int mavlink_fd)
if (x == NULL || y == NULL || z == NULL) { if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
/* clean up */
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR; res = ERROR;
return res; return res;
} }

4
src/modules/commander/module.mk
View File

@ -47,3 +47,7 @@ SRCS = commander.cpp \
baro_calibration.cpp \ baro_calibration.cpp \
rc_calibration.cpp \ rc_calibration.cpp \
airspeed_calibration.cpp airspeed_calibration.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

52
src/modules/dataman/dataman.c
View File

@ -1,10 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Jean Cyr
* Lorenz Meier
* Julian Oes
* Thomas Gubler
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -37,6 +33,11 @@
/** /**
* @file dataman.c * @file dataman.c
* DATAMANAGER driver. * DATAMANAGER driver.
*
* @author Jean Cyr
* @author Lorenz Meier
* @author Julian Oes
* @author Thomas Gubler
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -62,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t
__EXPORT int dm_clear(dm_item_t item); __EXPORT int dm_clear(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type); __EXPORT int dm_restart(dm_reset_reason restart_type);
/* Types of function calls supported by the worker task */ /** Types of function calls supported by the worker task */
typedef enum { typedef enum {
dm_write_func = 0, dm_write_func = 0,
dm_read_func, dm_read_func,
@ -71,11 +72,12 @@ typedef enum {
dm_number_of_funcs dm_number_of_funcs
} dm_function_t; } dm_function_t;
/* Work task work item */ /** Work task work item */
typedef struct { typedef struct {
sq_entry_t link; /**< list linkage */ sq_entry_t link; /**< list linkage */
sem_t wait_sem; sem_t wait_sem;
dm_function_t func; unsigned char first;
unsigned char func;
ssize_t result; ssize_t result;
union { union {
struct { struct {
@ -100,6 +102,8 @@ typedef struct {
}; };
} work_q_item_t; } work_q_item_t;
const size_t k_work_item_allocation_chunk_size = 8;
/* Usage statistics */ /* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs]; static unsigned g_func_counts[dm_number_of_funcs];
@ -177,9 +181,23 @@ create_work_item(void)
unlock_queue(&g_free_q); unlock_queue(&g_free_q);
/* If we there weren't any free items then obtain memory for a new one */ /* If we there weren't any free items then obtain memory for a new ones */
if (item == NULL) if (item == NULL) {
item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}
/* Update the queue size and potentially the maximum queue size */
g_free_q.size += k_work_item_allocation_chunk_size - 1;
if (g_free_q.size > g_free_q.max_size)
g_free_q.max_size = g_free_q.size;
unlock_queue(&g_free_q);
}
}
/* If we got one then lock the item*/ /* If we got one then lock the item*/
if (item) if (item)
@ -411,7 +429,7 @@ _clear(dm_item_t item)
return result; return result;
} }
/* Tell the data manager about the type of the last reset */ /** Tell the data manager about the type of the last reset */
static int static int
_restart(dm_reset_reason reason) _restart(dm_reset_reason reason)
{ {
@ -480,7 +498,7 @@ _restart(dm_reset_reason reason)
return result; return result;
} }
/* write to the data manager file */ /** Write to the data manager file */
__EXPORT ssize_t __EXPORT ssize_t
dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
{ {
@ -505,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return (ssize_t)enqueue_work_item_and_wait_for_result(work); return (ssize_t)enqueue_work_item_and_wait_for_result(work);
} }
/* Retrieve from the data manager file */ /** Retrieve from the data manager file */
__EXPORT ssize_t __EXPORT ssize_t
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{ {
@ -717,8 +735,8 @@ task_main(int argc, char *argv[])
for (;;) { for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break; break;
if (work->first)
free(work); free(work);
} }
destroy_q(&g_work_q); destroy_q(&g_work_q);
@ -736,7 +754,7 @@ start(void)
sem_init(&g_init_sema, 1, 0); sem_init(&g_init_sema, 1, 0);
/* start the worker thread */ /* start the worker thread */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
warn("task start failed"); warn("task start failed");
return -1; return -1;
} }

20
src/modules/dataman/dataman.h
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -46,7 +46,7 @@
extern "C" { extern "C" {
#endif #endif
/* Types of items that the data manager can store */ /** Types of items that the data manager can store */
typedef enum { typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
@ -56,7 +56,7 @@ extern "C" {
DM_KEY_NUM_KEYS /* Total number of item types defined */ DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t; } dm_item_t;
/* The maximum number of instances for each item type */ /** The maximum number of instances for each item type */
enum { enum {
DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@ -65,24 +65,24 @@ extern "C" {
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
}; };
/* Data persistence levels */ /** Data persistence levels */
typedef enum { typedef enum {
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
DM_PERSIST_VOLATILE /* Data does not survive resets */ DM_PERSIST_VOLATILE /* Data does not survive resets */
} dm_persitence_t; } dm_persitence_t;
/* The reason for the last reset */ /** The reason for the last reset */
typedef enum { typedef enum {
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */
DM_INIT_REASON_VOLATILE /* Data does not survive reset */ DM_INIT_REASON_VOLATILE /* Data does not survive reset */
} dm_reset_reason; } dm_reset_reason;
/* Maximum size in bytes of a single item instance */ /** Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 124 #define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */ /** Retrieve from the data manager store */
__EXPORT ssize_t __EXPORT ssize_t
dm_read( dm_read(
dm_item_t item, /* The item type to retrieve */ dm_item_t item, /* The item type to retrieve */
@ -91,7 +91,7 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */ size_t buflen /* Length in bytes of data to retrieve */
); );
/* write to the data manager store */ /** write to the data manager store */
__EXPORT ssize_t __EXPORT ssize_t
dm_write( dm_write(
dm_item_t item, /* The item type to store */ dm_item_t item, /* The item type to store */
@ -101,13 +101,13 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */ size_t buflen /* Length in bytes of data to retrieve */
); );
/* Erase all items of this type */ /** Erase all items of this type */
__EXPORT int __EXPORT int
dm_clear( dm_clear(
dm_item_t item /* The item type to clear */ dm_item_t item /* The item type to clear */
); );
/* Tell the data manager about the type of the last reset */ /** Tell the data manager about the type of the last reset */
__EXPORT int __EXPORT int
dm_restart( dm_restart(
dm_reset_reason restart_type /* The last reset type */ dm_reset_reason restart_type /* The last reset type */

2
src/modules/dataman/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = dataman MODULE_COMMAND = dataman
SRCS = dataman.c SRCS = dataman.c
MODULE_STACKSIZE = 1200

463
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp → src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file fw_att_pos_estimator_main.cpp * @file ekf_att_pos_estimator_main.cpp
* Implementation of the attitude and position estimator. * Implementation of the attitude and position estimator.
* *
* @author Paul Riseborough <p_riseborough@live.com.au> * @author Paul Riseborough <p_riseborough@live.com.au>
@ -73,6 +73,7 @@
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h> #include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h> #include <geo/geo.h>
@ -90,20 +91,19 @@
* *
* @ingroup apps * @ingroup apps
*/ */
extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis(); __EXPORT uint32_t millis();
static uint64_t last_run = 0; static uint64_t last_run = 0;
static uint64_t IMUmsec = 0; static uint64_t IMUmsec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis() uint32_t millis()
{ {
return IMUmsec; return IMUmsec;
} }
static void print_status();
class FixedwingEstimator class FixedwingEstimator
{ {
public: public:
@ -152,6 +152,7 @@ private:
int _params_sub; /**< notification of parameter updates */ int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */ int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub; int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _global_pos_pub; /**< global position */
@ -191,7 +192,13 @@ private:
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
bool _initialized; bool _initialized;
bool _baro_init;
bool _gps_initialized; bool _gps_initialized;
uint64_t _gps_start_time;
uint64_t _filter_start_time;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
int _mavlink_fd; int _mavlink_fd;
@ -201,6 +208,19 @@ private:
int32_t height_delay_ms; int32_t height_delay_ms;
int32_t mag_delay_ms; int32_t mag_delay_ms;
int32_t tas_delay_ms; int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
} _parameters; /**< local copies of interesting parameters */ } _parameters; /**< local copies of interesting parameters */
struct { struct {
@ -209,6 +229,19 @@ private:
param_t height_delay_ms; param_t height_delay_ms;
param_t mag_delay_ms; param_t mag_delay_ms;
param_t tas_delay_ms; param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf; AttPosEKF *_ekf;
@ -271,6 +304,8 @@ FixedwingEstimator::FixedwingEstimator() :
_vstatus_sub(-1), _vstatus_sub(-1),
_params_sub(-1), _params_sub(-1),
_manual_control_sub(-1), _manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
/* publications */ /* publications */
_att_pub(-1), _att_pub(-1),
@ -278,32 +313,68 @@ FixedwingEstimator::FixedwingEstimator() :
_local_pos_pub(-1), _local_pos_pub(-1),
_estimator_status_pub(-1), _estimator_status_pub(-1),
_att({}),
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
_sensor_combined({}),
#endif
_baro_ref(0.0f), _baro_ref(0.0f),
_baro_gps_offset(0.0f), _baro_gps_offset(0.0f),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")), _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")), _perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")), _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")), _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")), _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")), _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
/* states */ /* states */
_initialized(false), _initialized(false),
_baro_init(false),
_gps_initialized(false), _gps_initialized(false),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_mavlink_fd(-1), _mavlink_fd(-1),
_ekf(nullptr) _ekf(nullptr)
{ {
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); last_run = hrt_absolute_time();
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
_parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
_parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
_parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
_parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
_parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
_parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
_parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
_parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
_parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
_parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@ -317,6 +388,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) { if (fd > 0) {
res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
close(fd); close(fd);
if (res) {
warnx("G SCALE FAIL");
}
} }
fd = open(ACCEL_DEVICE_PATH, O_RDONLY); fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
@ -324,6 +399,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) { if (fd > 0) {
res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
close(fd); close(fd);
if (res) {
warnx("A SCALE FAIL");
}
} }
fd = open(MAG_DEVICE_PATH, O_RDONLY); fd = open(MAG_DEVICE_PATH, O_RDONLY);
@ -331,6 +410,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) { if (fd > 0) {
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
close(fd); close(fd);
if (res) {
warnx("M SCALE FAIL");
}
} }
} }
@ -368,6 +451,37 @@ FixedwingEstimator::parameters_update()
param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
if (_ekf) {
// _ekf->yawVarScale = 1.0f;
// _ekf->windVelSigma = 0.1f;
_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
_ekf->dVelBiasSigma = _parameters.abias_pnoise;
_ekf->magEarthSigma = _parameters.mage_pnoise;
_ekf->magBodySigma = _parameters.magb_pnoise;
// _ekf->gndHgtSigma = 0.02f;
_ekf->vneSigma = _parameters.velne_noise;
_ekf->vdSigma = _parameters.veld_noise;
_ekf->posNeSigma = _parameters.posne_noise;
_ekf->posDSigma = _parameters.posd_noise;
_ekf->magMeasurementSigma = _parameters.mag_noise;
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
}
return OK; return OK;
} }
@ -392,13 +506,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
estimator::g_estimator->task_main(); estimator::g_estimator->task_main();
} }
float dt = 0.0f; // time lapsed since last covariance prediction
void void
FixedwingEstimator::task_main() FixedwingEstimator::task_main()
{ {
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF(); _ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) { if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!"); errx(1, "failed allocating EKF filter - out of RAM!");
@ -412,6 +527,7 @@ FixedwingEstimator::task_main()
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
@ -431,26 +547,11 @@ FixedwingEstimator::task_main()
orb_set_interval(_sensor_combined_sub, 4); orb_set_interval(_sensor_combined_sub, 4);
#endif #endif
/* sets also parameters in the EKF object */
parameters_update(); parameters_update();
/* set initial filter state */
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[2]; struct pollfd fds[2];
@ -466,9 +567,8 @@ FixedwingEstimator::task_main()
fds[1].events = POLLIN; fds[1].events = POLLIN;
#endif #endif
hrt_abstime start_time = hrt_absolute_time();
bool newDataGps = false; bool newDataGps = false;
bool newHgtData = false;
bool newAdsData = false; bool newAdsData = false;
bool newDataMag = false; bool newDataMag = false;
@ -503,19 +603,52 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */ /* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll(); vehicle_status_poll();
bool accel_updated; bool accel_updated;
bool mag_updated; bool mag_updated;
hrt_abstime last_sensor_timestamp;
perf_count(_perf_gyro); perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/** /**
* PART ONE: COLLECT ALL DATA * PART ONE: COLLECT ALL DATA
**/ **/
hrt_abstime last_sensor_timestamp;
/* load local copies */ /* load local copies */
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@ -529,27 +662,46 @@ FixedwingEstimator::task_main()
} }
last_sensor_timestamp = _gyro.timestamp; last_sensor_timestamp = _gyro.timestamp;
_ekf.IMUmsec = _gyro.timestamp / 1e3f; IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f; float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp; last_run = _gyro.timestamp;
/* guard against too large deltaT's */ /* guard against too large deltaT's */
if (deltaT > 1.0f) if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f; deltaT = 0.01f;
}
// Always store data, independent of init status // Always store data, independent of init status
/* fill in last data set */ /* fill in last data set */
_ekf->dtIMU = deltaT; _ekf->dtIMU = deltaT;
_ekf->angRate.x = _gyro.x; if (isfinite(_gyro.x) &&
_ekf->angRate.y = _gyro.y; isfinite(_gyro.y) &&
_ekf->angRate.z = _gyro.z; isfinite(_gyro.z)) {
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
_ekf->accel.x = _accel.x; if (!_gyro_valid) {
_ekf->accel.y = _accel.y; lastAngRate = _ekf->angRate;
_ekf->accel.z = _accel.z; }
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate; _ekf->lastAngRate = angRate;
@ -565,6 +717,8 @@ FixedwingEstimator::task_main()
if (last_accel != _sensor_combined.accelerometer_timestamp) { if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true; accel_updated = true;
} else {
accel_updated = false;
} }
last_accel = _sensor_combined.accelerometer_timestamp; last_accel = _sensor_combined.accelerometer_timestamp;
@ -575,23 +729,43 @@ FixedwingEstimator::task_main()
IMUmsec = _sensor_combined.timestamp / 1e3f; IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */ /* guard against too large deltaT's */
if (deltaT > 1.0f || deltaT < 0.000001f) if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f; deltaT = 0.01f;
}
last_run = _sensor_combined.timestamp;
// Always store data, independent of init status // Always store data, independent of init status
/* fill in last data set */ /* fill in last data set */
_ekf->dtIMU = deltaT; _ekf->dtIMU = deltaT;
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; isfinite(_sensor_combined.gyro_rad_s[1]) &&
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; isfinite(_sensor_combined.gyro_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; if (!_gyro_valid) {
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; lastAngRate = _ekf->angRate;
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; }
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate; lastAngRate = _ekf->angRate;
@ -635,11 +809,13 @@ FixedwingEstimator::task_main()
perf_count(_perf_gps); perf_count(_perf_gps);
if (_gps.fix_type < 3) { if (_gps.fix_type < 3) {
gps_updated = false;
newDataGps = false; newDataGps = false;
} else { } else {
/* store time of valid GPS measurement */
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */ /* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
_ekf->ResetPosition(); _ekf->ResetPosition();
@ -660,6 +836,21 @@ FixedwingEstimator::task_main()
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f; _ekf->gpsHgt = _gps.alt / 1e3f;
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
newDataGps = true; newDataGps = true;
} }
@ -672,10 +863,17 @@ FixedwingEstimator::task_main()
if (baro_updated) { if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude - _baro_ref; _ekf->baroHgt = _baro.altitude;
// Could use a blend of GPS and baro alt data if desired if (!_baro_init) {
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; _baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
newHgtData = true;
} else {
newHgtData = false;
} }
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
@ -684,6 +882,8 @@ FixedwingEstimator::task_main()
if (mag_updated) { if (mag_updated) {
_mag_valid = true;
perf_count(_perf_mag); perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
@ -727,6 +927,8 @@ FixedwingEstimator::task_main()
*/ */
int check = _ekf->CheckAndBound(); int check = _ekf->CheckAndBound();
const char* ekfname = "[ekf] ";
switch (check) { switch (check) {
case 0: case 0:
/* all ok */ /* all ok */
@ -735,26 +937,38 @@ FixedwingEstimator::task_main()
{ {
const char* str = "NaN in states, resetting"; const char* str = "NaN in states, resetting";
warnx("%s", str); warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break; break;
} }
case 2: case 2:
{ {
const char* str = "stale IMU data, resetting"; const char* str = "stale IMU data, resetting";
warnx("%s", str); warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break; break;
} }
case 3: case 3:
{ {
const char* str = "switching dynamic / static state"; const char* str = "switching to dynamic state";
warnx("%s", str); warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str); mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break; break;
} }
default:
{
const char* str = "unknown reset condition";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
} }
// If non-zero, we got a problem // warn on fatal resets
if (check == 1) {
warnx("NUMERIC ERROR IN FILTER");
}
// If non-zero, we got a filter reset
if (check) { if (check) {
struct ekf_status_report ekf_report; struct ekf_status_report ekf_report;
@ -770,7 +984,7 @@ FixedwingEstimator::task_main()
rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
// Copy all states or at least all that we can fit // Copy all states or at least all that we can fit
int i = 0; unsigned i = 0;
unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
@ -786,6 +1000,23 @@ FixedwingEstimator::task_main()
} else { } else {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
} }
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
// Let the system re-initialize itself
continue;
} }
@ -793,52 +1024,70 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER * PART TWO: EXECUTE THE FILTER
**/ **/
// Wait long enough to ensure all sensors updated once if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
// XXX we rather want to check all updated
float initVelNED[3];
if (hrt_elapsed_time(&start_time) > 100000) { if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
if (!_gps_initialized && (_ekf->GPSstatus == 3)) { initVelNED[0] = _gps.vel_n_m_s;
_ekf->velNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s; initVelNED[2] = _gps.vel_d_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7; // GPS is in scaled integers, convert
double lon = _gps.lon * 1e-7; double lat = _gps.lat / 1.0e7;
float alt = _gps.alt * 1e-3; double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
_ekf->InitialiseFilter(_ekf->velNED); // Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_gps_offset = _baro_ref - _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection // Initialize projection
_local_pos.ref_lat = _gps.lat; _local_pos.ref_lat = lat;
_local_pos.ref_lon = _gps.lon; _local_pos.ref_lon = lon;
_local_pos.ref_alt = alt; _local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position; _local_pos.ref_timestamp = _gps.timestamp_position;
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
// XXX this is not multithreading safe
map_projection_init(&_pos_ref, lat, lon); map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
_gps_initialized = true; _gps_initialized = true;
} else if (!_ekf->statesInitialised) { } else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f; initVelNED[0] = 0.0f;
_ekf->velNED[2] = 0.0f; initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f; _ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f; _ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f; _ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1]; _ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
_local_pos.ref_alt = _baro_ref;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} }
} }
@ -872,10 +1121,10 @@ FixedwingEstimator::task_main()
// perform a covariance prediction if the total delta angle has exceeded the limit // perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update // or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
_ekf->CovariancePrediction(dt); _ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero(); _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero(); _ekf->summedDelVel.zero();
dt = 0.0f; dt = 0.0f;
} }
@ -926,9 +1175,9 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false; _ekf->fusePosData = false;
} }
if (newAdsData && _ekf->statesInitialised) { if (newHgtData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired // Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true; _ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays // recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
@ -1017,7 +1266,8 @@ FixedwingEstimator::task_main()
_local_pos.timestamp = last_sensor_timestamp; _local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = _ekf->states[7]; _local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8]; _local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9]; // XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_gps_offset;
_local_pos.vx = _ekf->states[4]; _local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5]; _local_pos.vy = _ekf->states[5];
@ -1050,6 +1300,8 @@ FixedwingEstimator::task_main()
_global_pos.lat = est_lat; _global_pos.lat = est_lat;
_global_pos.lon = est_lon; _global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec; _global_pos.time_gps_usec = _gps.time_gps_usec;
_global_pos.eph = _gps.eph_m;
_global_pos.epv = _gps.epv_m;
} }
if (_local_pos.v_xy_valid) { if (_local_pos.v_xy_valid) {
@ -1060,8 +1312,8 @@ FixedwingEstimator::task_main()
_global_pos.vel_e = 0.0f; _global_pos.vel_e = 0.0f;
} }
/* local pos alt is negative, change sign and add alt offset */ /* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
if (_local_pos.v_z_valid) { if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz; _global_pos.vel_d = _local_pos.vz;
@ -1102,7 +1354,7 @@ FixedwingEstimator::start()
ASSERT(_estimator_task == -1); ASSERT(_estimator_task == -1);
/* start the task */ /* start the task */
_estimator_task = task_spawn_cmd("fw_att_pos_estimator", _estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40, SCHED_PRIORITY_MAX - 40,
6000, 6000,
@ -1136,7 +1388,8 @@ FixedwingEstimator::print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
@ -1180,13 +1433,13 @@ int FixedwingEstimator::trip_nan() {
_ekf->states[5] = nan_val; _ekf->states[5] = nan_val;
usleep(100000); usleep(100000);
// warnx("tripping covariance #1 with NaN values"); warnx("tripping covariance #1 with NaN values");
// KH[2][2] = nan_val; // intermediate result used for covariance updates _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
// usleep(100000); usleep(100000);
// warnx("tripping covariance #2 with NaN values"); warnx("tripping covariance #2 with NaN values");
// KHP[5][5] = nan_val; // intermediate result used for covariance updates _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
// usleep(100000); usleep(100000);
warnx("tripping covariance #3 with NaN values"); warnx("tripping covariance #3 with NaN values");
_ekf->P[3][3] = nan_val; // covariance matrix _ekf->P[3][3] = nan_val; // covariance matrix
@ -1208,10 +1461,10 @@ int FixedwingEstimator::trip_nan() {
return ret; return ret;
} }
int fw_att_pos_estimator_main(int argc, char *argv[]) int ekf_att_pos_estimator_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1)
errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {

271
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c Normal file
View File

@ -0,0 +1,271 @@
/****************************************************************************
*
* 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 ekf_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* 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);
/**
* 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);
/**
* 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);
/**
* 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);
/**
* 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);
/**
* 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);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.0000001
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.0001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);

2609
src/modules/ekf_att_pos_estimator/estimator.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

129
src/modules/fw_att_pos_estimator/estimator.h → src/modules/ekf_att_pos_estimator/estimator.h
View File

@ -20,7 +20,7 @@ public:
float z; float z;
float length(void) const; float length(void) const;
Vector3f zero(void) const; void zero(void);
}; };
class Mat3f class Mat3f
@ -33,6 +33,7 @@ public:
Mat3f(); Mat3f();
void identity();
Mat3f transpose(void) const; Mat3f transpose(void) const;
}; };
@ -45,14 +46,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2); void swap_var(float &d1, float &d2);
const unsigned int n_states = 21; const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50; const unsigned int data_buffer_size = 50;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
// extern bool staticMode;
enum GPS_FIX { enum GPS_FIX {
GPS_FIX_NOFIX = 0, GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2, GPS_FIX_2D = 2,
@ -82,6 +78,88 @@ public:
AttPosEKF(); AttPosEKF();
~AttPosEKF(); ~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
}
struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
// Global variables // Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates
@ -96,6 +174,7 @@ public:
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@ -104,6 +183,10 @@ public:
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU; Vector3f dVelIMU;
Vector3f dAngIMU; Vector3f dAngIMU;
@ -115,26 +198,30 @@ public:
float velNED[3]; // North, East, Down velocity obs (m/s) float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m) float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m) float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m) float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad) float magDeclination; ///< magnetic declination
float lonRef; // WGS-84 longitude of reference point (rad) double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m) float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables // GPS input data variables
float gpsCourse; float gpsCourse;
float gpsVelD; float gpsVelD;
float gpsLat; double gpsLat;
float gpsLon; double gpsLon;
float gpsHgt; float gpsHgt;
uint8_t GPSstatus; uint8_t GPSstatus;
@ -148,11 +235,13 @@ public:
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
struct ekf_status_report current_ekf_state; struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error; struct ekf_status_report last_ekf_error;
@ -172,6 +261,10 @@ void FuseMagnetometer();
void FuseAirspeed(); void FuseAirspeed();
void FuseRangeFinder();
void FuseOpticalFlow();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@ -192,7 +285,7 @@ void StoreStates(uint64_t timestamp_ms);
* time-wise where valid states were updated and invalid remained at the old * time-wise where valid states were updated and invalid remained at the old
* value. * value.
*/ */
int RecallStates(float statesForFusion[n_states], uint64_t msec); int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates(); void ResetStoredStates();
@ -206,7 +299,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
@ -218,7 +311,7 @@ void OnGroundCheck();
void CovarianceInit(); void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3]); void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max); float ConstrainFloat(float val, float min, float max);
@ -243,7 +336,7 @@ void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report); bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err); void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]); void InitializeDynamic(float (&initvelNED)[3], float declination);
protected: protected:
@ -251,7 +344,7 @@ bool FilterHealthy();
void ResetHeight(void); void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
}; };

8
src/modules/fw_att_pos_estimator/module.mk → src/modules/ekf_att_pos_estimator/module.mk
View File

@ -32,11 +32,11 @@
############################################################################ ############################################################################
# #
# Main Attitude and Position Estimator for Fixed Wing Aircraft # Main EKF Attitude and Position Estimator
# #
MODULE_COMMAND = fw_att_pos_estimator MODULE_COMMAND = ekf_att_pos_estimator
SRCS = fw_att_pos_estimator_main.cpp \ SRCS = ekf_att_pos_estimator_main.cpp \
fw_att_pos_estimator_params.c \ ekf_att_pos_estimator_params.c \
estimator.cpp estimator.cpp

8
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -783,7 +783,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) { if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
} }
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@ -792,16 +792,16 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) { if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u); warnx("yaw_u %.4f", (double)yaw_u);
} }
/* throttle passed through */ /* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) { if (!isfinite(throttle_sp)) {
warnx("throttle_sp %.4f", throttle_sp); warnx("throttle_sp %.4f", (double)throttle_sp);
} }
} else { } else {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
} }
/* /*

2248
src/modules/fw_att_pos_estimator/estimator.cpp
View File

File diff suppressed because it is too large Load Diff

17
src/modules/mavlink/mavlink_main.cpp
View File

@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
/* If the wait until transmit flag is on, only transmit after we've received messages. /* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */ Otherwise, transmit all the time. */
if (instance->should_transmit()) { if (instance->should_transmit()) {
ssize_t ret = write(uart, ch, desired);
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
if (desired > buf_free) {
desired = buf_free;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) { if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection warnx("TX FAIL");
} }
} }
@ -1523,6 +1532,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{ {
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@ -2193,7 +2204,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf, task_spawn_cmd(buf,
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 1950,
(main_t)&Mavlink::start_helper, (main_t)&Mavlink::start_helper,
(const char **)argv); (const char **)argv);

1
src/modules/mavlink/mavlink_main.h
View File

@ -237,7 +237,6 @@ private:
orb_advert_t _mission_pub; orb_advert_t _mission_pub;
struct mission_s mission; struct mission_s mission;
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
MAVLINK_MODE _mode; MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id; uint8_t _mavlink_wpm_comp_id;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -949,7 +949,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
param.sched_priority = SCHED_PRIORITY_MAX - 40; param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param); (void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread; pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);

2
src/modules/mavlink/module.mk
View File

@ -48,3 +48,5 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1024

19
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,9 +32,13 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file mc_att_control_main.c * @file mc_att_control_main.cpp
* Multicopter attitude controller. * Multicopter attitude controller.
* *
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error. * The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
@ -506,6 +507,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* move yaw setpoint */ /* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
if (yaw_offs < - yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
}
_v_att_sp.R_valid = false; _v_att_sp.R_valid = false;
publish_att_sp = true; publish_att_sp = true;
} }
@ -817,7 +826,7 @@ MulticopterAttitudeControl::start()
_control_task = task_spawn_cmd("mc_att_control", _control_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2048, 2000,
(main_t)&MulticopterAttitudeControl::task_main_trampoline, (main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr); nullptr);

7
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -37,6 +34,10 @@
/** /**
* @file mc_att_control_params.c * @file mc_att_control_params.c
* Parameters for multicopter attitude controller. * Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <systemlib/param/param.h> #include <systemlib/param/param.h>

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1062,7 +1062,7 @@ MulticopterPositionControl::start()
_control_task = task_spawn_cmd("mc_pos_control", _control_task = task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2048, 2000,
(main_t)&MulticopterPositionControl::task_main_trampoline, (main_t)&MulticopterPositionControl::task_main_trampoline,
nullptr); nullptr);

2
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -35,6 +35,8 @@
/** /**
* @file mc_pos_control_params.c * @file mc_pos_control_params.c
* Multicopter position controller parameters. * Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <systemlib/param/param.h> #include <systemlib/param/param.h>

5
src/modules/navigator/geofence.cpp
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,6 +33,9 @@
/** /**
* @file geofence.cpp * @file geofence.cpp
* Provides functions for handling the geofence * Provides functions for handling the geofence
*
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include "geofence.h" #include "geofence.h"

5
src/modules/navigator/geofence.h
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,6 +33,9 @@
/** /**
* @file geofence.h * @file geofence.h
* Provides functions for handling the geofence * Provides functions for handling the geofence
*
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#ifndef GEOFENCE_H_ #ifndef GEOFENCE_H_

1
src/modules/navigator/geofence_params.c
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions

5
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,6 +33,9 @@
/** /**
* @file mission_feasibility_checker.cpp * @file mission_feasibility_checker.cpp
* Provides checks if mission is feasible given the navigation capabilities * Provides checks if mission is feasible given the navigation capabilities
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/ */
#include "mission_feasibility_checker.h" #include "mission_feasibility_checker.h"

6
src/modules/navigator/mission_feasibility_checker.h
View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,7 +33,11 @@
/** /**
* @file mission_feasibility_checker.h * @file mission_feasibility_checker.h
* Provides checks if mission is feasible given the navigation capabilities * Provides checks if mission is feasible given the navigation capabilities
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/ */
#ifndef MISSION_FEASIBILITY_CHECKER_H_ #ifndef MISSION_FEASIBILITY_CHECKER_H_
#define MISSION_FEASIBILITY_CHECKER_H_ #define MISSION_FEASIBILITY_CHECKER_H_

2
src/modules/navigator/module.mk
View File

@ -45,3 +45,5 @@ SRCS = navigator_main.cpp \
geofence_params.c geofence_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200

8
src/modules/navigator/navigator_main.cpp
View File

@ -1,10 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,7 +31,7 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file navigator_main.c * @file navigator_main.cpp
* Implementation of the main navigation state machine. * Implementation of the main navigation state machine.
* *
* Handles missions, geo fencing and failsafe navigation behavior. * Handles missions, geo fencing and failsafe navigation behavior.
@ -852,7 +848,7 @@ Navigator::start()
_navigator_task = task_spawn_cmd("navigator", _navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2048, 2000,
(main_t)&Navigator::task_main_trampoline, (main_t)&Navigator::task_main_trampoline,
nullptr); nullptr);

3
src/modules/navigator/navigator_mission.cpp
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -34,6 +33,8 @@
/** /**
* @file navigator_mission.cpp * @file navigator_mission.cpp
* Helper class to access missions * Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/ */
#include <string.h> #include <string.h>

3
src/modules/navigator/navigator_mission.h
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -34,6 +33,8 @@
/** /**
* @file navigator_mission.h * @file navigator_mission.h
* Helper class to access missions * Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/ */
#ifndef NAVIGATOR_MISSION_H #ifndef NAVIGATOR_MISSION_H

3
src/modules/navigator/navigator_params.c
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions

42
src/modules/navigator/navigator_state.h
View File

@ -1,8 +1,42 @@
/* /****************************************************************************
* navigator_state.h
* *
* Created on: 27.01.2014 * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: ton *
* 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 navigator_state.h
*
* Navigator state
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#ifndef NAVIGATOR_STATE_H_ #ifndef NAVIGATOR_STATE_H_

2
src/modules/position_estimator_inav/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = position_estimator_inav
SRCS = position_estimator_inav_main.c \ SRCS = position_estimator_inav_main.c \
position_estimator_inav_params.c \ position_estimator_inav_params.c \
inertial_filter.c inertial_filter.c
MODULE_STACKSIZE = 1200

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -135,7 +135,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
position_estimator_inav_thread_main, position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL); (argv) ? (const char **) &argv[2] : (const char **) NULL);
exit(0); exit(0);

7
src/modules/px4iofirmware/controls.c
View File

@ -291,6 +291,7 @@ controls_tick() {
/* set RC OK flag, as we got an update */ /* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */ /* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) { if (assigned_channels > 4) {
@ -336,6 +337,9 @@ controls_tick() {
PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK); PX4IO_P_STATUS_FLAGS_RC_OK);
/* flag raw RC as lost */
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
/* Mark all channels as invalid, as we just lost the RX */ /* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0; r_rc_valid = 0;
@ -405,8 +409,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
if (*num_values > PX4IO_RC_INPUT_CHANNELS) if (*num_values > PX4IO_RC_INPUT_CHANNELS)
*num_values = PX4IO_RC_INPUT_CHANNELS; *num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++) for (unsigned i = 0; i < *num_values; i++) {
values[i] = ppm_buffer[i]; values[i] = ppm_buffer[i];
}
/* clear validity */ /* clear validity */
ppm_last_valid_decode = 0; ppm_last_valid_decode = 0;

14
src/modules/px4iofirmware/protocol.h
View File

@ -142,6 +142,7 @@
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */
#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
@ -189,6 +190,8 @@
#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */
#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */
#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */
#else
#define PX4IO_P_SETUP_RELAYS_PAD 5
#endif #endif
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */
@ -208,15 +211,16 @@ enum { /* DSM bind states */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* 12 occupied by CRC */ /* storage space of 12 occupied by CRC */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state - this is a non-data write and
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
'armed' (PWM enabled) state */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */ /* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */

24
src/modules/px4iofirmware/registers.c
View File

@ -160,6 +160,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_PWM_ALTRATE] = 200,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
[PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_RELAYS] = 0,
#else
/* this is unused, but we will pad it for readability (the compiler pads it automatically) */
[PX4IO_P_SETUP_RELAYS_PAD] = 0,
#endif #endif
#ifdef ADC_VSERVO #ifdef ADC_VSERVO
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000, [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
@ -523,18 +526,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break; break;
case PX4IO_P_SETUP_PWM_DEFAULTRATE: case PX4IO_P_SETUP_PWM_DEFAULTRATE:
if (value < 50) if (value < 50) {
value = 50; value = 50;
if (value > 400) }
if (value > 400) {
value = 400; value = 400;
}
pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
break; break;
case PX4IO_P_SETUP_PWM_ALTRATE: case PX4IO_P_SETUP_PWM_ALTRATE:
if (value < 50) if (value < 50) {
value = 50; value = 50;
if (value > 400) }
if (value > 400) {
value = 400; value = 400;
}
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break; break;
@ -566,8 +573,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
} }
// check the magic value // check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC) if (value != PX4IO_REBOOT_BL_MAGIC) {
break; break;
}
// we schedule a reboot rather than rebooting // we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK // immediately to allow the IO board to ACK
@ -585,6 +593,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
} }
break; break;
case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
if (value > 650 && value < 2350) {
r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
}
break;
default: default:
return -1; return -1;
} }

2
src/modules/sdlog2/module.mk
View File

@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
SRCS = sdlog2.c \ SRCS = sdlog2.c \
logbuffer.c logbuffer.c
MODULE_STACKSIZE = 1200

16
src/modules/sdlog2/sdlog2.c
View File

@ -684,7 +684,7 @@ int sdlog2_thread_main(int argc, char *argv[])
case 'r': { case 'r': {
unsigned long r = strtoul(optarg, NULL, 10); unsigned long r = strtoul(optarg, NULL, 10);
if (r <= 0) { if (r == 0) {
r = 1; r = 1;
} }
@ -834,6 +834,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ESTM_s log_ESTM; struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR; struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN; struct log_VICN_s log_VICN;
struct log_GSN0_s log_GSN0;
struct log_GSN1_s log_GSN1;
} body; } body;
} log_msg = { } log_msg = {
LOG_PACKET_HEADER_INIT(0) LOG_PACKET_HEADER_INIT(0)
@ -982,6 +984,18 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
LOGBUFFER_WRITE_AND_COUNT(GPS); LOGBUFFER_WRITE_AND_COUNT(GPS);
/* log the SNR of each satellite for a detailed view of signal quality */
log_msg.msg_type = LOG_GSN0_MSG;
/* pick the smaller number so we do not overflow any of the arrays */
unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]);
unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr;
for (unsigned i = 0; i < sat_max_snr; i++) {
log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
}
LOGBUFFER_WRITE_AND_COUNT(GSN0);
} }
/* --- SENSOR COMBINED --- */ /* --- SENSOR COMBINED --- */

14
src/modules/sdlog2/sdlog2_messages.h
View File

@ -317,6 +317,18 @@ struct log_VICN_s {
float yaw; float yaw;
}; };
/* --- GSN0 - GPS SNR #0 --- */
#define LOG_GSN0_MSG 26
struct log_GSN0_s {
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
};
/* --- GSN1 - GPS SNR #1 --- */
#define LOG_GSN1_MSG 27
struct log_GSN1_s {
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/ /********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */ /* --- TIME - TIME STAMP --- */
@ -368,6 +380,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
/* system-level messages, ID >= 0x80 */ /* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */ /* FMT: don't write format of format message, it's useless */

2
src/modules/sensors/module.mk
View File

@ -40,3 +40,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5"
SRCS = sensors.cpp \ SRCS = sensors.cpp \
sensor_params.c sensor_params.c
MODULE_STACKSIZE = 1200

Some files were not shown because too many files have changed in this diff Show More