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

Merge branch 'master' into navigator_rewrite

Browse Source 
Conflicts:
	src/drivers/gps/gps.cpp
	src/drivers/gps/mtk.cpp
	src/modules/commander/commander.cpp
	src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
	src/modules/navigator/mission.cpp
	src/modules/navigator/mission.h
	src/modules/navigator/navigator_main.cpp
	src/modules/navigator/navigator_state.h
	src/modules/position_estimator_inav/position_estimator_inav_main.c
This commit is contained in:
Julian Oes 2014-05-26 20:19:11 +02:00
commit 063caba36b
173 changed files with 5312 additions and 1890 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/rc_mode_switch.odg
View File

Binary file not shown.

BIN
Documentation/rc_mode_switch.pdf
View File

Binary file not shown.

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
}

6
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -18,9 +18,9 @@ then
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAW_P 2.5
param set MC_YAWRATE_P 0.25
param set MC_YAWRATE_I 0.25
param set MC_YAWRATE_D 0.0
param set BAT_V_SCALING 0.00989

35
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d Normal file
View File

@ -0,0 +1,35 @@
#!nsh
#
# Steadidrone QU4D
#
# Thomas Gubler <thomasgubler@gmail.com>
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set BAT_N_CELLS 4
fi
set MIXER FMU_quad_w
set PWM_MIN 1210
set PWM_MAX 2100
set PWM_OUTPUTS 1234

2
ROMFS/px4fmu_common/init.d/11001_hexa_cox
View File

@ -1,7 +1,5 @@
#!nsh
#
# UNTESTED UNTESTED!
#
# Generic 10" Hexa coaxial geometry
#
# Lorenz Meier <lm@inf.ethz.ch>

2
ROMFS/px4fmu_common/init.d/2100_mpx_easystar
View File

@ -5,4 +5,4 @@
sh /etc/init.d/rc.fw_defaults
set MIXER easystar.mix
set MIXER easystar

5
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -195,6 +195,11 @@ then
sh /etc/init.d/10016_3dr_iris
fi
if param compare SYS_AUTOSTART 10017
then
sh /etc/init.d/10017_steadidrone_qu4d
fi
#
# Hexa Coaxial
#

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_LAND_T -1
param set NAV_ACCEPT_RAD 50
param set RC_SCALE_ROLL 1
param set RC_SCALE_PITCH 1
fi

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

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

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

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

20
ROMFS/px4fmu_common/init.d/rcS
View File

@ -419,16 +419,6 @@ then
mavlink start $MAVLINK_FLAGS
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#
navigator start
#
# Sensors, Logging, GPS
#
@ -550,6 +540,16 @@ then
fi
fi
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#
navigator start
#
# Generic setup (autostart ID not found)
#

3
Tools/fix_code_style.sh
View File

@ -16,5 +16,6 @@ astyle \
--ignore-exclude-errors-x \
--lineend=linux \
--exclude=EASTL \
--add-brackets \
--add-brackets \
--max-code-length=120 \
$*

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)

14
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -48,6 +48,16 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
# Check if the right version of the toolchain is available
#
CROSSDEV_VER_SUPPORTED = 4.7
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)
ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)))
$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
endif
# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
MAXOPTIMIZATION ?= -O3
@ -76,7 +86,7 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
# Pick the right set of flags for the architecture.
#
@ -265,7 +275,7 @@ define SYM_TO_BIN
$(Q) $(OBJCOPY) -O binary $1 $2
endef
# Take the raw binary $1 and make it into an object file $2.
# Take the raw binary $1 and make it into an object file $2.
# The symbol $3 points to the beginning of the file, and $3_len
# gives its length.
#

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

42
nuttx-configs/aerocore/include/nsh_romfsimg.h Normal file
View File

@ -0,0 +1,42 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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.
*
****************************************************************************/
/**
* nsh_romfsetc.h
*
* This file is a stub for 'make export' purposes; the actual ROMFS
* must be supplied by the library client.
*/
extern unsigned char romfs_img[];
extern unsigned int romfs_img_len;

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 =

17
src/examples/flow_position_control/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
# modification, are permitted provided that the following conditions
@ -12,7 +14,7 @@
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
@ -31,11 +33,10 @@
#
############################################################################
#
# Build multirotor position control
#
# Path to example in apps/examples containing the user_start entry point
MODULE_COMMAND = flow_position_control
CONFIGURED_APPS += examples/nsh
SRCS = flow_position_control_main.c \
flow_position_control_params.c
# The NSH application library
CONFIGURED_APPS += nshlib
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.
*/

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

@ -418,7 +418,7 @@ CONFIG_PREALLOC_TIMERS=50
# Stack and heap information
#
CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_USERMAIN_STACKSIZE=3500
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y
#
# USART1 Configuration
#
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART1_RXBUFSIZE=256
CONFIG_USART1_TXBUFSIZE=128
CONFIG_USART1_BAUD=57600
CONFIG_USART1_BITS=8
CONFIG_USART1_PARITY=0
@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y
#
# UART5 Configuration
#
CONFIG_UART5_RXBUFSIZE=512
CONFIG_UART5_TXBUFSIZE=512
CONFIG_UART5_RXBUFSIZE=256
CONFIG_UART5_TXBUFSIZE=128
CONFIG_UART5_BAUD=57600
CONFIG_UART5_BITS=8
CONFIG_UART5_PARITY=0
@ -540,8 +540,8 @@ CONFIG_UART5_2STOP=0
#
# USART6 Configuration
#
CONFIG_USART6_RXBUFSIZE=512
CONFIG_USART6_TXBUFSIZE=512
CONFIG_USART6_RXBUFSIZE=128
CONFIG_USART6_TXBUFSIZE=64
CONFIG_USART6_BAUD=57600
CONFIG_USART6_BITS=8
CONFIG_USART6_PARITY=0

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

@ -119,7 +119,7 @@ int ardrone_interface_main(int argc, char *argv[])
ardrone_interface_task = task_spawn_cmd("ardrone_interface",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
1100,
ardrone_interface_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);

1
src/drivers/ardrone_interface/module.mk
View File

@ -38,3 +38,4 @@
MODULE_COMMAND = ardrone_interface
SRCS = ardrone_interface.c \
ardrone_motor_control.c
MODULE_STACKSIZE = 1200

71
src/drivers/blinkm/blinkm.cpp
View File

@ -194,6 +194,26 @@ private:
bool systemstate_run;
int vehicle_status_sub_fd;
int vehicle_control_mode_sub_fd;
int vehicle_gps_position_sub_fd;
int actuator_armed_sub_fd;
int safety_sub_fd;
int num_of_cells;
int detected_cells_runcount;
int t_led_color[8];
int t_led_blink;
int led_thread_runcount;
int led_interval;
bool topic_initialized;
bool detected_cells_blinked;
bool led_thread_ready;
int num_of_used_sats;
void setLEDColor(int ledcolor);
static void led_trampoline(void *arg);
void led();
@ -265,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) :
led_color_7(LED_OFF),
led_color_8(LED_OFF),
led_blink(LED_NOBLINK),
systemstate_run(false)
systemstate_run(false),
vehicle_status_sub_fd(-1),
vehicle_control_mode_sub_fd(-1),
vehicle_gps_position_sub_fd(-1),
actuator_armed_sub_fd(-1),
safety_sub_fd(-1),
num_of_cells(0),
detected_cells_runcount(0),
t_led_color({0}),
t_led_blink(0),
led_thread_runcount(0),
led_interval(1000),
topic_initialized(false),
detected_cells_blinked(false),
led_thread_ready(true),
num_of_used_sats(0)
{
memset(&_work, 0, sizeof(_work));
}
@ -382,31 +417,6 @@ void
BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
static int actuator_armed_sub_fd;
static int safety_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
static int led_thread_runcount=0;
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_control_mode = 0;
static int no_data_actuator_armed = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
static bool detected_cells_blinked = false;
static bool led_thread_ready = true;
int num_of_used_sats = 0;
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 250);
@ -494,6 +504,11 @@ BlinkM::led()
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
int no_data_vehicle_status = 0;
int no_data_vehicle_control_mode = 0;
int no_data_actuator_armed = 0;
int no_data_vehicle_gps_position = 0;
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
@ -638,11 +653,11 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;

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

143
src/examples/flow_position_control/flow_position_control_params.h → src/drivers/boards/aerocore/aerocore_led.c
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,68 +31,91 @@
*
****************************************************************************/
/**
* @file aerocore_led.c
*
* AeroCore LED backend.
*/
#include <nuttx/config.h>
#include <stdbool.h>
#include "stm32.h"
#include "board_config.h"
#include <arch/board/board.h>
/*
* @file flow_position_control_params.h
*
* Parameters for position controller
* 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
#include <systemlib/param/param.h>
__EXPORT void led_init()
{
stm32_configgpio(GPIO_LED0);
stm32_configgpio(GPIO_LED1);
}
struct flow_position_control_params {
float pos_p;
float pos_d;
float height_p;
float height_i;
float height_d;
float height_rate;
float height_min;
float height_max;
float thrust_feedforward;
float limit_speed_x;
float limit_speed_y;
float limit_height_error;
float limit_thrust_int;
float limit_thrust_upper;
float limit_thrust_lower;
float limit_yaw_step;
float manual_threshold;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
__EXPORT void led_on(int led)
{
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, true);
break;
struct flow_position_control_param_handles {
param_t pos_p;
param_t pos_d;
param_t height_p;
param_t height_i;
param_t height_d;
param_t height_rate;
param_t height_min;
param_t height_max;
param_t thrust_feedforward;
param_t limit_speed_x;
param_t limit_speed_y;
param_t limit_height_error;
param_t limit_thrust_int;
param_t limit_thrust_upper;
param_t limit_thrust_lower;
param_t limit_yaw_step;
param_t manual_threshold;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
case 1:
stm32_gpiowrite(GPIO_LED1, true);
break;
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct flow_position_control_param_handles *h);
default:
warnx("LED ID not recognized\n");
}
}
/**
* Update all parameters
*
*/
int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p);
__EXPORT void led_off(int led)
{
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, false);
break;
case 1:
stm32_gpiowrite(GPIO_LED1, false);
break;
default:
warnx("LED ID not recognized\n");
}
}
__EXPORT void led_toggle(int led)
{
switch (led) {
case 0:
if (stm32_gpioread(GPIO_LED0))
stm32_gpiowrite(GPIO_LED0, false);
else
stm32_gpiowrite(GPIO_LED0, true);
break;
case 1:
if (stm32_gpioread(GPIO_LED1))
stm32_gpiowrite(GPIO_LED1, false);
else
stm32_gpiowrite(GPIO_LED1, true);
break;
default:
warnx("LED ID not recognized\n");
}
}

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_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
#define PX4_SPI_BUS_SENSORS 1
/*
* Use these in place of the spi_dev_e enumeration to
* select a specific SPI device on SPI1

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_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
#define PX4_SPI_BUS_SENSORS 1
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2

10
src/drivers/drv_gpio.h
View File

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

4
src/drivers/drv_gps.h
View File

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

10
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -154,8 +154,9 @@ ETSAirspeed::collect()
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa == 0) {
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
uint16_t diff_pres_pa;
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
@ -165,10 +166,10 @@ ETSAirspeed::collect()
return -1;
}
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// Track maximum differential pressure measured (so we can work out top speed).
@ -183,6 +184,7 @@ ETSAirspeed::collect()
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.temperature = -1000.0f;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;

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

@ -53,6 +53,8 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
/* FrSky sensor hub data IDs */
#define FRSKY_ID_GPS_ALT_BP 0x01
#define FRSKY_ID_TEMP1 0x02
@ -192,17 +194,12 @@ void frsky_send_frame1(int uart)
}
/**
* Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
* Formats the decimal latitude/longitude to the required degrees/minutes.
*/
static float frsky_format_gps(float dec)
{
float dms_deg = (int) dec;
float dec_deg = dec - dms_deg;
float dms_min = (int) (dec_deg * 60);
float dec_min = (dec_deg * 60) - dms_min;
float dms_sec = dec_min * 60;
return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
float dm_deg = (int) dec;
return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
}
/**
@ -230,9 +227,9 @@ void frsky_send_frame2(int uart)
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat));
lat = frsky_format_gps(fabsf(global_pos.lat));
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(global_pos.lon));
lon = frsky_format_gps(fabsf(global_pos.lon));
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
* 25.0f / 46.0f;

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",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
2000,
frsky_telemetry_thread_main,
(const char **)argv);

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

@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.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
* modification, are permitted provided that the following conditions
@ -56,6 +56,7 @@
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
@ -63,6 +64,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <board_config.h>
#include "ubx.h"
#include "mtk.h"
@ -76,12 +79,6 @@
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class GPS : public device::CDev
{
public:
@ -209,7 +206,8 @@ GPS::init()
goto out;
/* 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) {
warnx("task start failed: %d", errno);
@ -421,7 +419,14 @@ GPS::task_main()
void
GPS::cmd_reset()
{
//XXX add reset?
#ifdef GPIO_GPS_NRESET
warnx("Toggling GPS reset pin");
stm32_configgpio(GPIO_GPS_NRESET);
stm32_gpiowrite(GPIO_GPS_NRESET, 0);
usleep(100);
stm32_gpiowrite(GPIO_GPS_NRESET, 1);
warnx("Toggled GPS reset pin");
#endif
}
void
@ -443,10 +448,10 @@ GPS::print_info()
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", _report.eph, _report.epv);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);

4
src/drivers/gps/gps_helper.cpp
View File

@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
return _rate_vel;
}
float
void
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
_interval_rate_start = hrt_absolute_time();
}
float
void
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);

8
src/drivers/gps/gps_helper.h
View File

@ -46,13 +46,17 @@
class GPS_Helper
{
public:
GPS_Helper() {};
virtual ~GPS_Helper() {};
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
float reset_update_rates();
float store_update_rates();
void reset_update_rates();
void store_update_rates();
protected:
uint8_t _rate_count_lat_lon;

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

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

10
src/drivers/gps/mtk.cpp
View File

@ -249,12 +249,18 @@ MTK::handle_message(gps_mtk_packet_t &packet)
warnx("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
// Indicate this data is not usable and bail out
_gps_position->eph_m = 1000.0f;
_gps_position->epv_m = 1000.0f;
_gps_position->fix_type = 0;
return;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
_gps_position->epv = 0.0; //unknown in mtk custom mode
_gps_position->eph = packet.hdop / 100.0f; // from cm to m
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;

47
src/drivers/gps/ubx.cpp
View File

@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: configuration failed: RATE");
warnx("CFG FAIL: RATE");
return 1;
}
@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: configuration failed: NAV5");
warnx("CFG FAIL: NAV5");
return 1;
}
@ -194,35 +194,42 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV POSLLH");
warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SOL");
warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV VELNED");
warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SVINFO");
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
warnx("ubx: poll error");
warnx("poll error");
return -1;
} else if (ret == 0) {
@ -310,7 +317,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
warnx("ubx: timeout - no useful messages");
warnx("timeout - no useful messages");
return -1;
}
}
@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
return 1; // message received successfully
} else {
warnx("ubx: checksum wrong");
warnx("checksum wrong");
decode_init();
return -1;
}
@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
warnx("ubx: buffer full");
warnx("buffer full");
decode_init();
return -1;
}
@ -566,6 +573,24 @@ UBX::handle_message()
break;
}
case UBX_CLASS_MON: {
switch (_message_id) {
case UBX_MESSAGE_MON_HW: {
struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
_gps_position->noise_per_ms = p->noisePerMS;
_gps_position->jamming_indicator = p->jamInd;
ret = 1;
break;
}
default:
break;
}
}
default:
break;
}

28
src/drivers/gps/ubx.h
View File

@ -56,6 +56,7 @@
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
@ -72,6 +73,8 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_MON_HW 0x09
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
@ -210,6 +213,27 @@ typedef struct {
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
struct gps_bin_mon_hw_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t __reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t __reserved3;
uint32_t pinIrq;
uint32_t pulLH;
uint32_t pullL;
};
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
@ -319,7 +343,7 @@ typedef enum {
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
@ -383,7 +407,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
uint8_t _disable_cmd_last;
hrt_abstime _disable_cmd_last;
};
#endif /* UBX_H_ */

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

@ -122,7 +122,7 @@ private:
actuator_controls_s _controls;
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,

10
src/drivers/hmc5883/hmc5883.cpp
View File

@ -158,6 +158,7 @@ private:
int _class_instance;
orb_advert_t _mag_topic;
orb_advert_t _subsystem_pub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@ -324,7 +325,9 @@ HMC5883::HMC5883(int bus) :
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_mag_topic(-1),
_subsystem_pub(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
@ -1137,13 +1140,12 @@ int HMC5883::check_calibration()
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
static orb_advert_t pub = -1;
if (!(_pub_blocked)) {
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
if (_subsystem_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}

32
src/drivers/hott/messages.cpp
View File

@ -51,6 +51,8 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
static int _esc_sub = -1;
static orb_advert_t _esc_pub;
struct esc_status_s _esc;
static bool _home_position_set = false;
static double _home_lat = 0.0d;
@ -82,8 +83,6 @@ init_sub_messages(void)
void
init_pub_messages(void)
{
memset(&_esc, 0, sizeof(_esc));
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
}
void
@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
size_t size = sizeof(msg);
memset(&msg, 0, size);
memcpy(&msg, buffer, size);
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
// Publish it.
esc.timestamp = hrt_absolute_time();
esc.esc_count = 1;
esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
esc.esc[0].esc_temperature = msg.temperature1 - 20;
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
/* announce the esc if needed, just publish else */
if (_esc_pub > 0) {
orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
} else {
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
}
// Publish it.
_esc.esc_count = 1;
_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
_esc.esc[0].esc_temperature = msg.temperature1 - 20;
_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
}
void

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

@ -34,6 +34,9 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
*
* Note: With the exception of the self-test feature, the ST L3G4200D is
* also supported by this driver.
*/
#include <nuttx/config.h>
@ -89,9 +92,11 @@ static const int ERROR = -1;
#define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM_H 0xD7
#define WHO_I_AM 0xD4
#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
#define ADDR_CTRL_REG1 0x20
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
@ -166,9 +171,14 @@ static const int ERROR = -1;
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
#define L3GD20_DEFAULT_RATE 760
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
@ -216,6 +226,9 @@ private:
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
/* true if an L3G4200D is detected */
bool _is_l3g4200d;
/**
* Start automatic measurement.
*/
@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_topic(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_is_l3g4200d(false)
{
// enable debug() calls
_debug_enabled = true;
@ -413,14 +427,7 @@ L3GD20::probe()
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
#else
#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
#endif
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
success = true;
}
@ -430,6 +437,13 @@ L3GD20::probe()
success = true;
}
/* Detect the L3G4200D used on AeroCore */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
_is_l3g4200d = true;
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
success = true;
}
if (success)
return OK;
@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
if (_is_l3g4200d) {
return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
}
return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
@ -683,23 +700,26 @@ L3GD20::set_samplerate(unsigned frequency)
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
if (frequency == 0)
frequency = 760;
frequency = _is_l3g4200d ? 800 : 760;
/* use limits good for H or non-H models */
/*
* Use limits good for H or non-H models. Rates are slightly different
* for L3G4200D part but register settings are the same.
*/
if (frequency <= 100) {
_current_rate = 95;
_current_rate = _is_l3g4200d ? 100 : 95;
bits |= RATE_95HZ_LP_25HZ;
} else if (frequency <= 200) {
_current_rate = 190;
_current_rate = _is_l3g4200d ? 200 : 190;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
_current_rate = 380;
_current_rate = _is_l3g4200d ? 400 : 380;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
_current_rate = 760;
_current_rate = _is_l3g4200d ? 800 : 760;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -EINVAL;
@ -772,7 +792,7 @@ L3GD20::reset()
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_samplerate(0); // 760Hz
set_samplerate(0); // 760Hz or 800Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
@ -971,7 +991,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;

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

@ -1793,7 +1793,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");

38
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -92,8 +92,20 @@
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
class MK : public device::I2C
{
@ -154,8 +166,10 @@ private:
actuator_controls_s _controls;
MotorData_t Motor[MAX_MOTORS];
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
@ -195,24 +209,6 @@ const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0};
int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
MotorData_t Motor[MAX_MOTORS];
namespace
{

1
src/drivers/mpu6000/mpu6000.cpp
View File

@ -1380,7 +1380,6 @@ MPU6000_gyro::init()
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
out:
return ret;
}

4
src/drivers/ms5611/ms5611.cpp
View File

@ -753,8 +753,8 @@ MS5611::print_info()
printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF);
printf("P: %.3f\n", _P);
printf("T: %.3f\n", _T);
printf("P: %.3f\n", (double)_P);
printf("T: %.3f\n", (double)_T);
printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
printf("factory_setup %u\n", _prom.factory_setup);

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

@ -117,7 +117,7 @@ private:
device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf)
{
return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :

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

@ -92,6 +92,7 @@ public:
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
MODE_8PWM,
};
PX4FMU();
virtual ~PX4FMU();
@ -113,6 +114,9 @@ private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
static const unsigned _max_actuators = 8;
#endif
Mode _mode;
unsigned _pwm_default_rate;
@ -149,7 +153,7 @@ private:
unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
@ -203,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
/* AeroCore breaks out User GPIOs on J11 */
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0},
{GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0},
{GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0},
{GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0},
{GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0},
#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@ -382,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
break;
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
debug("MODE_8PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
/* XXX magic numbers */
up_pwm_servo_init(0xff);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
#endif
case MODE_NONE:
debug("MODE_NONE");
@ -559,13 +591,12 @@ PX4FMU::task_main()
}
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
* input at 50Hz */
int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
} else if (ret == 0) {
@ -603,6 +634,9 @@ PX4FMU::task_main()
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
@ -614,11 +648,9 @@ PX4FMU::task_main()
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/* last resort: catch NaN and INF */
if ((i >= outputs.noutputs) ||
!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
@ -630,6 +662,7 @@ PX4FMU::task_main()
uint16_t pwm_limited[num_outputs];
/* the PWM limit call takes care of out of band errors and constrains */
pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
@ -758,6 +791,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
@ -987,6 +1023,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_SET(7):
case PWM_SERVO_SET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@ -1014,6 +1059,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
@ -1041,12 +1095,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET_RATEGROUP(6):
case PWM_SERVO_GET_RATEGROUP(7):
#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
@ -1092,6 +1156,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_6PWM);
break;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
case 8:
set_mode(MODE_8PWM);
break;
#endif
default:
ret = -EINVAL;
@ -1182,10 +1251,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
unsigned count = len / 2;
uint16_t values[6];
#ifdef CONFIG_ARCH_BOARD_AEROCORE
if (count > 8) {
// we have at most 8 outputs
count = 8;
}
#else
if (count > 6) {
// we have at most 6 outputs
count = 6;
}
#endif
// allow for misaligned values
memcpy(values, buffer, count * 2);
@ -1459,6 +1535,9 @@ fmu_new_mode(PortMode new_mode)
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
servo_mode = PX4FMU::MODE_6PWM;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
servo_mode = PX4FMU::MODE_8PWM;
#endif
break;
@ -1777,7 +1856,7 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);

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

@ -4,3 +4,5 @@
MODULE_COMMAND = fmu
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
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MODULE_STACKSIZE = 1200

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

@ -529,6 +529,11 @@ PX4IO::~PX4IO()
if (_interface != nullptr)
delete _interface;
/* deallocate perfs */
perf_free(_perf_update);
perf_free(_perf_write);
perf_free(_perf_chan_count);
g_dev = nullptr;
}
@ -576,8 +581,10 @@ PX4IO::init()
ASSERT(_task == -1);
sys_restart_param = param_find("SYS_RESTART_TYPE");
/* Indicate restart type is unknown */
param_set(sys_restart_param, &sys_restart_val);
if (sys_restart_param != PARAM_INVALID) {
/* Indicate restart type is unknown */
param_set(sys_restart_param, &sys_restart_val);
}
/* do regular cdev init */
ret = CDev::init();
@ -789,7 +796,12 @@ PX4IO::init()
}
/* 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) {
debug("task start failed: %d", errno);
@ -984,13 +996,17 @@ PX4IO::task_main()
int32_t failsafe_param_val;
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);
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");
if (failsafe_param_val > 0) {
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 +1464,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
/* we don't have the status bits, so input_source has to be set elsewhere */
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
/*
@ -1456,8 +1472,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
input_rc.timestamp_publication = hrt_absolute_time();
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
if (ret != OK)
@ -1469,23 +1483,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*/
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
if (channel_count != _rc_chan_count)
/* limit the channel count */
if (channel_count > RC_INPUT_MAX_CHANNELS) {
channel_count = RC_INPUT_MAX_CHANNELS;
}
/* count channel count changes to identify signal integrity issues */
if (channel_count != _rc_chan_count) {
perf_count(_perf_chan_count);
}
_rc_chan_count = channel_count;
input_rc.timestamp_publication = hrt_absolute_time();
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
input_rc.channel_count = channel_count;
/* rc_lost has to be set before the call to this function */
if (!input_rc.rc_lost && !input_rc.rc_failsafe)
if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
_rc_last_valid = input_rc.timestamp_publication;
}
input_rc.timestamp_last_signal = _rc_last_valid;
/* FIELDS NOT SET HERE */
/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@ -1493,8 +1522,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
return ret;
}
input_rc.channel_count = channel_count;
memcpy(input_rc.values, &regs[prolog], channel_count * 2);
/* last thing set are the actual channel values as 16 bit values */
for (unsigned i = 0; i < channel_count; i++) {
input_rc.values[i] = regs[prolog + i];
}
return ret;
}

2
src/drivers/px4io/px4io_serial.cpp
View File

@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
if (_rx_dma_status == _dma_status_waiting) {
/* verify that the received packet is complete */
unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);

7
src/drivers/sf0x/sf0x.cpp
View File

@ -254,9 +254,6 @@ SF0X::~SF0X()
int
SF0X::init()
{
int ret = ERROR;
unsigned i = 0;
/* do regular cdev init */
if (CDev::init() != OK) {
goto out;
@ -594,7 +591,7 @@ SF0X::collect()
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (int i = 0; i < (lend - 2); i++) {
for (unsigned i = 0; i < (lend - 2); i++) {
if (_linebuf[i] == '\n') {
char buf[sizeof(_linebuf)];
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
@ -795,7 +792,7 @@ const int ERROR = -1;
SF0X *g_dev;
void start();
void start(const char *port);
void stop();
void test();
void reset();

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

@ -145,7 +145,7 @@ private:
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
_channel_count(0),
_samples(nullptr),
_to_system_power(0)
@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
#endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
#endif
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");

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

@ -94,7 +94,7 @@
#elif HRT_TIMER == 3
# define HRT_TIMER_BASE STM32_TIM3_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM3
# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN
# if CONFIG_STM32_TIM3
@ -141,7 +141,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP
# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN
# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN
# if CONFIG_STM32_TIM10
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
# endif
@ -150,7 +150,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM
# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN
# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN
# if CONFIG_STM32_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
#define PPM_DEBUG 0
#if PPM_DEBUG
/* PPM edge history */
__EXPORT uint16_t ppm_edge_history[32];
unsigned ppm_edge_next;
@ -361,6 +364,7 @@ unsigned ppm_edge_next;
/* PPM pulse history */
__EXPORT uint16_t ppm_pulse_history[32];
unsigned ppm_pulse_next;
#endif
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status)
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
#if PPM_DEBUG
ppm_edge_history[ppm_edge_next++] = width;
if (ppm_edge_next >= 32)
ppm_edge_next = 0;
#endif
/*
* if this looks like a start pulse, then push the last set of values
@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status)
interval = count - ppm.last_mark;
ppm.last_mark = count;
#if PPM_DEBUG
ppm_pulse_history[ppm_pulse_next++] = interval;
if (ppm_pulse_next >= 32)
ppm_pulse_next = 0;
#endif
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))

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

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

613
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -1,613 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 flow_position_control.c
*
* Optical flow position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <math.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_position_control_params.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int flow_position_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int flow_position_control_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);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
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_spawn_cmd().
*/
int flow_position_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
printf("flow position control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("flow_position_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,
flow_position_control_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)
printf("\tflow position control app is running\n");
else
printf("\tflow position control app not started\n");
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int
flow_position_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[fpc] started");
uint32_t counter = 0;
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_advert_t speed_sp_pub;
bool speed_setpoint_adverted = false;
/* parameters init*/
struct flow_position_control_params params;
struct flow_position_control_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);
/* init flow sum setpoint */
float flow_sp_sumx = 0.0f;
float flow_sp_sumy = 0.0f;
/* init yaw setpoint */
float yaw_sp = 0.0f;
/* init height setpoint */
float height_sp = params.height_min;
/* height controller states */
bool start_phase = true;
bool landing_initialized = false;
float landing_thrust_start = 0.0f;
/* states */
float integrated_h_error = 0.0f;
float last_local_pos_z = 0.0f;
bool update_flow_sp_sumx = false;
bool update_flow_sp_sumy = false;
uint64_t last_time = 0.0f;
float dt = 0.0f; // s
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
/* wait for first attitude msg to be sure all data are available */
if (sensors_ready)
{
/* polling */
struct pollfd fds[2] = {
{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a position update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow position control] no filtered flow updates\n");
}
else
{
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
}
/* only run controller if position/speed changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
status_changed = true;
/* calc dt */
if(last_time == 0)
{
last_time = hrt_absolute_time();
continue;
}
dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
last_time = hrt_absolute_time();
/* update flow sum setpoint */
if (update_flow_sp_sumx)
{
flow_sp_sumx = filtered_flow.sumx;
update_flow_sp_sumx = false;
}
if (update_flow_sp_sumy)
{
flow_sp_sumy = filtered_flow.sumy;
update_flow_sp_sumy = false;
}
/* calc new bodyframe speed setpoints */
float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
float speed_limit_height_factor = height_sp; // the settings are for 1 meter
/* overwrite with rc input if there is any */
if(isfinite(manual_pitch) && isfinite(manual_roll))
{
if(fabsf(manual_pitch) > params.manual_threshold)
{
speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
update_flow_sp_sumx = true;
}
if(fabsf(manual_roll) > params.manual_threshold)
{
speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
update_flow_sp_sumy = true;
}
}
/* limit speed setpoints */
if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
{
speed_sp.vx = speed_body_x;
}
else
{
if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
}
if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
{
speed_sp.vy = speed_body_y;
}
else
{
if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
}
/* manual yaw change */
if(isfinite(manual_yaw) && isfinite(manual.throttle))
{
if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
{
yaw_sp += manual_yaw * params.limit_yaw_step;
/* modulo for rotation -pi +pi */
if(yaw_sp < -M_PI_F)
yaw_sp = yaw_sp + M_TWOPI_F;
else if(yaw_sp > M_PI_F)
yaw_sp = yaw_sp - M_TWOPI_F;
}
}
/* forward yaw setpoint */
speed_sp.yaw_sp = yaw_sp;
/* manual height control
* 0-20%: thrust linear down
* 20%-40%: down
* 40%-60%: stabilize altitude
* 60-100%: up
*/
float thrust_control = 0.0f;
if (isfinite(manual.throttle))
{
if (start_phase)
{
/* control start thrust with stick input */
if (manual.throttle < 0.4f)
{
/* first 40% for up to feedforward */
thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
}
else
{
/* second 60% for up to feedforward + 10% */
thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
}
/* exit start phase if setpoint is reached */
if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
{
start_phase = false;
/* switch to stabilize */
thrust_control = params.thrust_feedforward;
}
}
else
{
if (manual.throttle < 0.2f)
{
/* landing initialization */
if (!landing_initialized)
{
/* consider last thrust control to avoid steps */
landing_thrust_start = speed_sp.thrust_sp;
landing_initialized = true;
}
/* set current height as setpoint to avoid steps */
if (-local_pos.z > params.height_min)
height_sp = -local_pos.z;
else
height_sp = params.height_min;
/* lower 20% stick range controls thrust down */
thrust_control = manual.throttle / 0.2f * landing_thrust_start;
/* assume ground position here */
if (thrust_control < 0.1f)
{
/* reset integral if on ground */
integrated_h_error = 0.0f;
/* switch to start phase */
start_phase = true;
/* reset height setpoint */
height_sp = params.height_min;
}
}
else
{
/* stabilized mode */
landing_initialized = false;
/* calc new thrust with PID */
float height_error = (local_pos.z - (-height_sp));
/* update height setpoint if needed*/
if (manual.throttle < 0.4f)
{
/* down */
if (height_sp > params.height_min + params.height_rate &&
fabsf(height_error) < params.limit_height_error)
height_sp -= params.height_rate * dt;
}
if (manual.throttle > 0.6f)
{
/* up */
if (height_sp < params.height_max &&
fabsf(height_error) < params.limit_height_error)
height_sp += params.height_rate * dt;
}
/* instead of speed limitation, limit height error (downwards) */
if(height_error > params.limit_height_error)
height_error = params.limit_height_error;
else if(height_error < -params.limit_height_error)
height_error = -params.limit_height_error;
integrated_h_error = integrated_h_error + height_error;
float integrated_thrust_addition = integrated_h_error * params.height_i;
if(integrated_thrust_addition > params.limit_thrust_int)
integrated_thrust_addition = params.limit_thrust_int;
if(integrated_thrust_addition < -params.limit_thrust_int)
integrated_thrust_addition = -params.limit_thrust_int;
float height_speed = last_local_pos_z - local_pos.z;
float thrust_diff = height_error * params.height_p - height_speed * params.height_d;
thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
/* add attitude component
* F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
*/
// // TODO problem with attitude
// if (att.R_valid && att.R[2][2] > 0)
// thrust_control = thrust_control / att.R[2][2];
/* set thrust lower limit */
if(thrust_control < params.limit_thrust_lower)
thrust_control = params.limit_thrust_lower;
}
}
/* set thrust upper limit */
if(thrust_control > params.limit_thrust_upper)
thrust_control = params.limit_thrust_upper;
}
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
speed_sp.thrust_sp = thrust_control; //manual.throttle;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
{
if(speed_setpoint_adverted)
{
orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
}
else
{
speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
speed_setpoint_adverted = true;
}
}
else
{
warnx("NaN in flow position controller!");
}
}
else
{
/* in manual or stabilized state just reset speed and flow sum setpoint */
//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
status_changed = false;
speed_sp.vx = 0.0f;
speed_sp.vy = 0.0f;
flow_sp_sumx = filtered_flow.sumx;
flow_sp_sumy = filtered_flow.sumy;
if(isfinite(att.yaw))
{
yaw_sp = att.yaw;
speed_sp.yaw_sp = att.yaw;
}
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
counter++;
}
else
{
/* sensors not ready waiting for first attitude msg */
/* polling */
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
/* wait for a flow msg, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
}
}
}
}
mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
thread_running = false;
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_local_position_sub);
close(armed_sub);
close(control_mode_sub);
close(manual_control_setpoint_sub);
close(speed_sp_pub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
return 0;
}

124
src/examples/flow_position_control/flow_position_control_params.c
View File

@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 flow_position_control_params.c
*/
#include "flow_position_control_params.h"
/* controller parameters */
// Position control P gain
PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
// Position control D / damping gain
PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
// Altitude control P gain
PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
// Altitude control I (integrator) gain
PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
// Altitude control D gain
PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
// Altitude control rate limiter
PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
// Altitude control minimum altitude
PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
// Altitude control maximum altitude (higher than 1.5m is untested)
PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
// Altitude control feed forward throttle - adjust to the
// throttle position (0..1) where the copter hovers in manual flight
PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f);
PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f);
PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f);
PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f);
PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f);
PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f);
int parameters_init(struct flow_position_control_param_handles *h)
{
/* PID parameters */
h->pos_p = param_find("FPC_POS_P");
h->pos_d = param_find("FPC_POS_D");
h->height_p = param_find("FPC_H_P");
h->height_i = param_find("FPC_H_I");
h->height_d = param_find("FPC_H_D");
h->height_rate = param_find("FPC_H_RATE");
h->height_min = param_find("FPC_H_MIN");
h->height_max = param_find("FPC_H_MAX");
h->thrust_feedforward = param_find("FPC_T_FFWD");
h->limit_speed_x = param_find("FPC_L_S_X");
h->limit_speed_y = param_find("FPC_L_S_Y");
h->limit_height_error = param_find("FPC_L_H_ERR");
h->limit_thrust_int = param_find("FPC_L_TH_I");
h->limit_thrust_upper = param_find("FPC_L_TH_U");
h->limit_thrust_lower = param_find("FPC_L_TH_L");
h->limit_yaw_step = param_find("FPC_L_YAW_STEP");
h->manual_threshold = param_find("FPC_MAN_THR");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p)
{
param_get(h->pos_p, &(p->pos_p));
param_get(h->pos_d, &(p->pos_d));
param_get(h->height_p, &(p->height_p));
param_get(h->height_i, &(p->height_i));
param_get(h->height_d, &(p->height_d));
param_get(h->height_rate, &(p->height_rate));
param_get(h->height_min, &(p->height_min));
param_get(h->height_max, &(p->height_max));
param_get(h->thrust_feedforward, &(p->thrust_feedforward));
param_get(h->limit_speed_x, &(p->limit_speed_x));
param_get(h->limit_speed_y, &(p->limit_speed_y));
param_get(h->limit_height_error, &(p->limit_height_error));
param_get(h->limit_thrust_int, &(p->limit_thrust_int));
param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
param_get(h->limit_yaw_step, &(p->limit_yaw_step));
param_get(h->manual_threshold, &(p->manual_threshold));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}

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

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

@ -38,3 +38,5 @@
MODULE_COMMAND = px4_daemon_app
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",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
2000,
px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);

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

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

21
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -63,11 +63,22 @@ ECL_PitchController::ECL_PitchController() :
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
}
ECL_PitchController::~ECL_PitchController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
@ -123,6 +134,14 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
isfinite(airspeed_max) && isfinite(scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();

4
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@ -51,12 +51,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_PitchController //XXX: create controller superclass
{
public:
ECL_PitchController();
~ECL_PitchController();
float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
@ -126,6 +129,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_PITCH_CONTROLLER_H

23
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -61,10 +61,21 @@ ECL_RollController::ECL_RollController() :
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
}
ECL_RollController::~ECL_RollController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll_setpoint) && isfinite(roll))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
/* Calculate error */
float roll_error = roll_setpoint - roll;
@ -86,6 +97,14 @@ float ECL_RollController::control_bodyrate(float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
isfinite(airspeed_min) && isfinite(airspeed_max) &&
isfinite(scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
@ -122,8 +141,8 @@ float ECL_RollController::control_bodyrate(float pitch,
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);

4
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@ -51,12 +51,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_RollController //XXX: create controller superclass
{
public:
ECL_RollController();
~ECL_RollController();
float control_attitude(float roll_setpoint, float roll);
float control_bodyrate(float pitch,
@ -117,6 +120,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_ROLL_CONTROLLER_H

20
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -60,12 +60,25 @@ ECL_YawController::ECL_YawController() :
_bodyrate_setpoint(0.0f),
_coordinated_min_speed(1.0f)
{
perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
}
ECL_YawController::~ECL_YawController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
isfinite(pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
@ -103,6 +116,13 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
isfinite(airspeed_max) && isfinite(scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();

4
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@ -50,12 +50,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_YawController //XXX: create controller superclass
{
public:
ECL_YawController();
~ECL_YawController();
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
@ -118,6 +121,7 @@ private:
float _rate_setpoint;
float _bodyrate_setpoint;
float _coordinated_min_speed;
perf_counter_t _nonfinite_input_perf;
};

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

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

25
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.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,9 +32,12 @@
****************************************************************************/
/*
* @file attitude_estimator_ekf_main.c
* @file attitude_estimator_ekf_main.cpp
*
* Extended Kalman Filter for Attitude Estimation.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@ -111,7 +112,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
@ -265,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
@ -273,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
@ -286,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl;
@ -381,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@ -392,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
@ -444,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
@ -497,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);

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

@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
codegen/rtGetNaN.c \
codegen/norm.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.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,6 +34,9 @@
/*
* @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).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* 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.
*
* 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[])
{

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 \
attitude_estimator_so3_params.c
MODULE_STACKSIZE = 1200

66
src/modules/commander/accelerometer_calibration.cpp
View File

@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3,3> board_rotation;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
}
if (old_done_count != done_count)
if (old_done_count != done_count) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done)
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f)
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i])
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 0; // [ g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 1; // [ -g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 2; // [ 0, g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 3; // [ 0, -g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
return 4; // [ 0, 0, g ]
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ]
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
accel_sum[i] += sensor.accelerometer_m_s2[i];
}
count++;
@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
continue;
}
if (errcount > samples_num / 10)
if (errcount > samples_num / 10) {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f)
return ERROR; // Singular matrix
if (det == 0.0f) {
return ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
}
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {

6
src/modules/commander/airspeed_calibration.cpp
View File

@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd)
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */

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

@ -221,7 +221,7 @@ void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@ -233,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
int commander_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
@ -248,7 +249,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3000,
2950,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@ -261,8 +262,9 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running)
if (!thread_running) {
errx(0, "commander already stopped");
}
thread_should_exit = true;
@ -304,8 +306,9 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
@ -364,20 +367,22 @@ void print_status()
static orb_advert_t status_pub;
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
}
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
@ -414,43 +419,61 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
if (hil_ret == OK) {
ret = true;
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_res = main_state_transition(status, MAIN_STATE_ACRO);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
}
}
}
}
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@ -463,24 +486,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
} else {
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
} else {
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
break;
@ -504,7 +531,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
@ -523,6 +550,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
if (use_current) {
/* use current position */
if (status->condition_global_position_valid) {
@ -566,6 +594,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@ -579,6 +608,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
@ -612,9 +642,10 @@ int commander_thread_main(int argc, char *argv[])
char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL";
main_states_str[1] = "SEATBELT";
main_states_str[2] = "EASY";
main_states_str[1] = "ALTCTL";
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
main_states_str[4] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
@ -705,7 +736,7 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_t 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;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@ -860,6 +891,7 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
@ -900,6 +932,7 @@ int commander_thread_main(int argc, char *argv[])
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
@ -925,9 +958,11 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
} else {
eph_epv_good = true;
}
@ -935,17 +970,19 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
} else {
eph_epv_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@ -972,7 +1009,26 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
local_eph_good = false;
} else {
local_eph_good = true;
}
} else {
if (local_position.eph < eph_epv_threshold) {
local_eph_good = true;
} else {
local_eph_good = false;
}
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
@ -1050,8 +1106,9 @@ int commander_thread_main(int argc, char *argv[])
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
if (last_idle_time > 0)
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
if (last_idle_time > 0) {
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
}
last_idle_time = system_load.tasks[0].total_runtime;
@ -1127,22 +1184,22 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
transition_result_t res; // store all transitions results here
transition_result_t arming_res; // store all transitions results here
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
arming_res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
(status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@ -1155,16 +1212,16 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@ -1177,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter = 0;
}
if (res == TRANSITION_CHANGED) {
if (arming_res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
@ -1185,24 +1242,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
} else if (res == TRANSITION_DENIED) {
} else if (arming_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status, &sp_man);
transition_result_t main_res = set_main_state_rc(&status, &sp_man);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
if (main_res == TRANSITION_CHANGED) {
tune_positive(armed.armed);
} else if (res == TRANSITION_DENIED) {
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
@ -1215,7 +1272,8 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* MISSION switch */
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAVIGATION_STATE_LOITER;
@ -1225,7 +1283,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state = NAVIGATION_STATE_MISSION;
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAVIGATION_STATE_MISSION;
}
@ -1241,27 +1299,20 @@ int commander_thread_main(int argc, char *argv[])
if (armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_NOT_CHANGED) {
if (auto_res == TRANSITION_NOT_CHANGED) {
last_auto_state_valid = hrt_absolute_time();
}
/* still invalid state after the timeout interval, execute failsafe */
if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) {
if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
if (auto_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
}
} else if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
} else {
status.set_nav_state = NAVIGATION_STATE_MISSION;
@ -1269,47 +1320,37 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* failsafe for manual modes */
transition_result_t res = TRANSITION_DENIED;
transition_result_t manual_res = TRANSITION_DENIED;
if (!status.condition_landed) {
/* vehicle is not landed, try to perform RTL */
res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND");
}
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
}
if (res == TRANSITION_DENIED) {
if (manual_res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
}
if (res == TRANSITION_DENIED) {
if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
} else if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
} else {
status.set_nav_state = NAVIGATION_STATE_RTL;
}
}
}
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
/* flight termination in manual mode if assist switch is on POSCTL position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@ -1323,8 +1364,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
status_changed = true;
}
}
/* check which state machines for changes, clear "changed" flag */
@ -1341,7 +1383,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
@ -1353,7 +1395,7 @@ int commander_thread_main(int argc, char *argv[])
home.z = local_position.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
@ -1367,6 +1409,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
was_armed = armed.armed;
if (main_state_changed) {
@ -1530,21 +1573,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
if (leds_counter % 20 == 0) {
led_toggle(LED_BLUE);
}
} else {
/* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_BLUE);
}
}
#endif
/* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) {
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_AMBER);
}
} else {
led_off(LED_AMBER);
@ -1565,30 +1611,35 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break;
case SWITCH_POS_OFF: // MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
if (sp_man->acro_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_ACRO);
} else {
res = main_state_transition(status, MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_MIDDLE: // ASSISTED
if (sp_man->assisted_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_EASY);
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT
print_reject_mode(status, "EASY");
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL");
}
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->assisted_switch != SWITCH_POS_ON) {
print_reject_mode(status, "SEATBELT");
if (sp_man->posctl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
@ -1603,9 +1654,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
// else fallback to SEATBELT (EASY likely will not work too)
// else fallback to ALTCTL (POSCTL likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -1651,7 +1702,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@ -1662,7 +1713,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@ -1675,6 +1726,18 @@ set_control_mode()
case MAIN_STATE_AUTO:
navigator_enabled = true;
break;
case MAIN_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
default:
break;
@ -1758,7 +1821,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
@ -1772,7 +1835,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
/* this needs additional hints to the user - so let other messages pass and be spoken */
mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
tune_negative(true);
break;
@ -1808,8 +1872,9 @@ void *commander_low_prio_loop(void *arg)
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for thread_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@ -1824,8 +1889,9 @@ void *commander_low_prio_loop(void *arg)
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO)
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
continue;
}
/* only handle low-priority commands here */
switch (cmd.command) {
@ -1903,6 +1969,7 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
@ -1917,10 +1984,12 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
if (calib_ret == OK) {
tune_positive(true);
else
} else {
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
@ -1940,11 +2009,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@ -1960,11 +2031,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@ -1974,8 +2047,8 @@ void *commander_low_prio_loop(void *arg)
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
/* handled in the IO driver */
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */

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

@ -113,17 +113,22 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune(int tune) {
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
@ -198,7 +206,7 @@ int led_init()
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);
/* we consider the amber led mandatory */
@ -244,22 +252,25 @@ int led_off(int led)
void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
}
}
void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
}
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;

24
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
ut_assert("tranisition: manual to seatbelt",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
// MANUAL to ALTCTRL, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
ut_assert("transition: manual to easy",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
// MANUAL to EASY, invalid local position.
// MANUAL to POSCTRL, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
new_main_state = MAIN_STATE_POSCTL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

18
src/modules/commander/gyro_calibration.cpp
View File

@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else {
poll_errcount++;
@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
warn("WARNING: failed to apply new offsets for gyro");
}
close(fd);
@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
uint64_t last_time = hrt_absolute_time();
@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
if (mag < -M_PI_F) mag += 2 * M_PI_F;
if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
if (diff < -M_PI_F) diff += 2 * M_PI_F;
if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
baseline_integral += diff;
mag_last = mag;

47
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;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd)
if (x == NULL || y == NULL || z == NULL) {
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;
return res;
}
} else {
/* exit */
return ERROR;
@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
if (calibration_counter % (calibration_maxcount / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
}
} else {
poll_errcount++;
@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd)
}
}
if (x != NULL)
if (x != NULL) {
free(x);
}
if (y != NULL)
if (y != NULL) {
free(y);
}
if (z != NULL)
if (z != NULL) {
free(z);
}
if (res == OK) {
/* apply calibration and set parameters */
@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);

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

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

5
src/modules/commander/px4_custom_mode.h
View File

@ -12,9 +12,10 @@
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {

6
src/modules/commander/rc_calibration.cpp
View File

@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */

164
src/modules/commander/state_machine_helper.cpp
View File

@ -75,38 +75,38 @@ static bool failsafe_state_changed = true;
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char* state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
static const char *state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == status->arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED) {
static const char* errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
return ret;
}
@ -215,7 +222,11 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
@ -226,7 +237,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
@ -301,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
break;
@ -320,6 +331,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
@ -331,26 +343,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;

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

@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Jean Cyr
* Lorenz Meier
* Julian Oes
* Thomas Gubler
* 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
@ -37,6 +33,11 @@
/**
* @file dataman.c
* DATAMANAGER driver.
*
* @author Jean Cyr
* @author Lorenz Meier
* @author Julian Oes
* @author Thomas Gubler
*/
#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_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 {
dm_write_func = 0,
dm_read_func,
@ -71,11 +72,12 @@ typedef enum {
dm_number_of_funcs
} dm_function_t;
/* Work task work item */
/** Work task work item */
typedef struct {
sq_entry_t link; /**< list linkage */
sem_t wait_sem;
dm_function_t func;
unsigned char first;
unsigned char func;
ssize_t result;
union {
struct {
@ -100,6 +102,8 @@ typedef struct {
};
} work_q_item_t;
const size_t k_work_item_allocation_chunk_size = 8;
/* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs];
@ -177,9 +181,23 @@ create_work_item(void)
unlock_queue(&g_free_q);
/* If we there weren't any free items then obtain memory for a new one */
if (item == NULL)
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
/* If we there weren't any free items then obtain memory for a new ones */
if (item == NULL) {
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 (item)
@ -411,7 +429,7 @@ _clear(dm_item_t item)
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
_restart(dm_reset_reason reason)
{
@ -480,7 +498,7 @@ _restart(dm_reset_reason reason)
return result;
}
/* write to the data manager file */
/** Write to the data manager file */
__EXPORT ssize_t
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);
}
/* Retrieve from the data manager file */
/** Retrieve from the data manager file */
__EXPORT ssize_t
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 (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break;
free(work);
if (work->first)
free(work);
}
destroy_q(&g_work_q);
@ -736,7 +754,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* 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");
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
* modification, are permitted provided that the following conditions
@ -46,7 +46,7 @@
extern "C" {
#endif
/* Types of items that the data manager can store */
/** Types of items that the data manager can store */
typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
@ -56,7 +56,7 @@ extern "C" {
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
/* The maximum number of instances for each item type */
/** The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@ -65,24 +65,24 @@ extern "C" {
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
};
/* Data persistence levels */
/** Data persistence levels */
typedef enum {
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
DM_PERSIST_VOLATILE /* Data does not survive resets */
} dm_persitence_t;
/* The reason for the last reset */
/** The reason for the last reset */
typedef enum {
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */
DM_INIT_REASON_VOLATILE /* Data does not survive reset */
} 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
/* Retrieve from the data manager store */
/** Retrieve from the data manager store */
__EXPORT ssize_t
dm_read(
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 */
);
/* write to the data manager store */
/** write to the data manager store */
__EXPORT ssize_t
dm_write(
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 */
);
/* Erase all items of this type */
/** Erase all items of this type */
__EXPORT int
dm_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
dm_restart(
dm_reset_reason restart_type /* The last reset type */

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

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

56
src/modules/ekf_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.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
@ -97,6 +97,7 @@ __EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint64_t IMUmsec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
{
@ -194,6 +195,7 @@ private:
bool _baro_init;
bool _gps_initialized;
uint64_t _gps_start_time;
uint64_t _filter_start_time;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
@ -246,7 +248,7 @@ private:
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
float _airspeed_filtered;
/**
* Update our local parameter cache.
@ -272,7 +274,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace estimator
@ -306,6 +308,8 @@ FixedwingEstimator::FixedwingEstimator() :
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
/* publications */
_att_pub(-1),
@ -336,13 +340,13 @@ FixedwingEstimator::FixedwingEstimator() :
_baro_gps_offset(0.0f),
/* performance counters */
_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
_loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
_perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
_perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
_perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
/* states */
_initialized(false),
@ -509,14 +513,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
estimator::g_estimator->task_main();
}
float dt = 0.0f; // time lapsed since last covariance prediction
void
FixedwingEstimator::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
@ -642,6 +646,7 @@ FixedwingEstimator::task_main()
_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;
@ -719,6 +724,8 @@ FixedwingEstimator::task_main()
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
@ -809,7 +816,6 @@ FixedwingEstimator::task_main()
perf_count(_perf_gps);
if (_gps.fix_type < 3) {
gps_updated = false;
newDataGps = false;
} else {
@ -1025,7 +1031,7 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER
**/
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
float initVelNED[3];
@ -1042,9 +1048,9 @@ FixedwingEstimator::task_main()
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_gps_offset = gps_alt - _baro.altitude;
_baro_gps_offset = _baro_ref - _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
@ -1061,14 +1067,14 @@ FixedwingEstimator::task_main()
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = _baro_ref + _baro_gps_offset;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
map_projection_init(&_pos_ref, lat, lon);
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", _ekf->baroHgt, _ekf->hgtMea);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
_gps_initialized = true;
@ -1084,6 +1090,10 @@ FixedwingEstimator::task_main()
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_local_pos.ref_alt = _baro_ref;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
}
}
@ -1263,7 +1273,8 @@ FixedwingEstimator::task_main()
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9] + _baro_gps_offset;
// 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.vy = _ekf->states[5];
@ -1324,8 +1335,8 @@ FixedwingEstimator::task_main()
_global_pos.vel_e = 0.0f;
}
/* local pos alt is negative, change sign and add alt offset */
_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
@ -1400,7 +1411,8 @@ FixedwingEstimator::print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)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("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]);

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