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

Merge remote-tracking branch 'upstream/master' into geo

Browse Source 
Conflicts:
	src/modules/commander/commander.cpp
	src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
This commit is contained in:
Thomas Gubler
2014-05-22 13:25:52 +02:00
parent a6a2efb651 9ea0b21899
commit 05648d80d2
225 changed files with 8138 additions and 10988 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/rc_mode_switch.odg
BIN
View File
Binary file not shown.
Documentation/rc_mode_switch.pdf
BIN
View File
Binary file not shown.
Images/aerocore.prototype
+12
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
}
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+35
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
ROMFS/px4fmu_common/init.d/11001_hexa_cox
-2
View File
@@ -1,7 +1,5 @@
#!nsh
#
# UNTESTED UNTESTED!
#
# Generic 10" Hexa coaxial geometry
#
# Lorenz Meier <lm@inf.ethz.ch>
ROMFS/px4fmu_common/init.d/rc.autostart
+5
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
#
ROMFS/px4fmu_common/init.d/rc.fw_apps
+1 -1
View File
@@ -6,7 +6,7 @@
#
# Start the attitude and position estimator
#
fw_att_pos_estimator start
ekf_att_pos_estimator start
#
# Start attitude controller
ROMFS/px4fmu_common/init.d/rc.fw_defaults
-2
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
ROMFS/px4fmu_common/init.d/rc.logging
+1 -1
View File
@@ -8,7 +8,7 @@ then
if ver hwcmp PX4FMU_V1
then
echo "Start sdlog2 at 50Hz"
sdlog2 start -r 50 -a -b 8 -t
sdlog2 start -r 50 -a -b 4 -t
else
echo "Start sdlog2 at 200Hz"
sdlog2 start -r 200 -a -b 16 -t
ROMFS/px4fmu_common/init.d/rc.mc_apps
+1
View File
@@ -5,6 +5,7 @@
#
attitude_estimator_ekf start
#ekf_att_pos_estimator start
position_estimator_inav start
mc_att_control start
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+6
View File
@@ -35,6 +35,12 @@ then
param set MPC_TILTMAX_AIR 45.0
param set MPC_TILTMAX_LND 15.0
param set MPC_LAND_SPEED 1.0
param set PE_VELNE_NOISE 0.5
param set PE_VELNE_NOISE 0.7
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
fi
set PWM_RATE 400
ROMFS/px4fmu_common/init.d/rc.usb
+7 -3
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
ROMFS/px4fmu_common/init.d/rcS
+10 -10
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)
#
Tools/fix_code_style.sh
+2 -1
View File
@@ -16,5 +16,6 @@ astyle \
--ignore-exclude-errors-x \
--lineend=linux \
--exclude=EASTL \
--add-brackets \
--add-brackets \
--max-code-length=120 \
$*
makefiles/board_aerocore.mk
+11
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
makefiles/config_aerocore_default.mk
+125
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)
makefiles/config_px4fmu-v1_default.mk
+1 -1
View File
@@ -70,7 +70,7 @@ MODULES += modules/gpio_led
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/fw_att_pos_estimator
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator
makefiles/config_px4fmu-v2_default.mk
+1 -1
View File
@@ -79,7 +79,7 @@ MODULES += modules/gpio_led
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/fw_att_pos_estimator
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
nuttx-configs/aerocore/include/board.h
+263
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 */
src/modules/fixedwing_att_control/fixedwing_att_control_rate.h → nuttx-configs/aerocore/include/nsh_romfsimg.h
+9 -15
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.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,16 +31,12 @@
*
****************************************************************************/
/* @file Fixed Wing Attitude Rate Control */
/**
* nsh_romfsetc.h
*
* This file is a stub for 'make export' purposes; the actual ROMFS
* must be supplied by the library client.
*/
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
#define FIXEDWING_ATT_CONTROL_RATE_H_
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators);
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
extern unsigned char romfs_img[];
extern unsigned int romfs_img_len;
nuttx-configs/aerocore/nsh/Make.defs
+179
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 =
src/modules/fixedwing_att_control/module.mk → nuttx-configs/aerocore/nsh/appconfig
+9 -9
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,12 +33,10 @@
#
############################################################################
#
# Fixedwing Attitude Control application
#
# Path to example in apps/examples containing the user_start entry point
MODULE_COMMAND = fixedwing_att_control
CONFIGURED_APPS += examples/nsh
SRCS = fixedwing_att_control_main.c \
fixedwing_att_control_att.c \
fixedwing_att_control_rate.c
# The NSH application library
CONFIGURED_APPS += nshlib
CONFIGURED_APPS += system/readline
nuttx-configs/aerocore/nsh/defconfig
+950
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
#
src/modules/att_pos_estimator_ekf/module.mk → nuttx-configs/aerocore/nsh/setenv.sh
Regular → Executable
+36 -11
View File
@@ -1,6 +1,8 @@
############################################################################
#!/bin/bash
# configs/aerocore/nsh/setenv.sh
#
# 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.
#
@@ -29,14 +31,37 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Full attitude / position Extended Kalman Filter
#
if [ "$_" = "$0" ] ; then
echo "You must source this script, not run it!" 1>&2
exit 1
fi
MODULE_COMMAND = att_pos_estimator_ekf
WD=`pwd`
if [ ! -x "setenv.sh" ]; then
echo "This script must be executed from the top-level NuttX build directory"
exit 1
fi
SRCS = kalman_main.cpp \
KalmanNav.cpp \
params.c
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}"
nuttx-configs/aerocore/scripts/ld.script
+150
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) }
}
src/modules/position_estimator/module.mk → nuttx-configs/aerocore/src/Makefile
+50 -10
View File
@@ -1,6 +1,8 @@
############################################################################
# configs/aerocore/src/Makefile
#
# 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,14 +33,52 @@
#
############################################################################
#
# Makefile to build the position estimator
#
-include $(TOPDIR)/Make.defs
MODULE_COMMAND = position_estimator
CFLAGS += -I$(TOPDIR)/sched
# XXX this should be converted to a deamon, its a pretty bad example app
MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT
MODULE_STACKSIZE = 4096
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
SRCS = position_estimator_main.c
nuttx-configs/aerocore/src/empty.c
+4
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.
*/
nuttx-configs/px4fmu-v1/nsh/defconfig
+7 -7
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
src/drivers/ardrone_interface/ardrone_interface.c
+1 -1
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);
src/drivers/ardrone_interface/module.mk
+1
View File
@@ -38,3 +38,4 @@
MODULE_COMMAND = ardrone_interface
SRCS = ardrone_interface.c \
ardrone_motor_control.c
MODULE_STACKSIZE = 1200
src/drivers/blinkm/blinkm.cpp
+43 -28
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;
src/drivers/boards/aerocore/aerocore_init.c
+282
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;
}
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c → src/drivers/boards/aerocore/aerocore_led.c
+73 -69
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,86 +32,90 @@
****************************************************************************/
/**
* @file fw_att_pos_estimator_params.c
* @file aerocore_led.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* AeroCore LED backend.
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
#include <stdbool.h>
#include "stm32.h"
#include "board_config.h"
#include <arch/board/board.h>
/*
* Estimator parameters, accessible via MAVLink
*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
__EXPORT void led_init()
{
stm32_configgpio(GPIO_LED0);
stm32_configgpio(GPIO_LED1);
}
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
__EXPORT void led_on(int led)
{
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, true);
break;
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
case 1:
stm32_gpiowrite(GPIO_LED1, true);
break;
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
default:
warnx("LED ID not recognized\n");
}
}
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
__EXPORT void led_off(int led)
{
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, false);
break;
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
case 1:
stm32_gpiowrite(GPIO_LED1, false);
break;
default:
warnx("LED ID not recognized\n");
}
}
__EXPORT void led_toggle(int led)
{
switch (led) {
case 0:
if (stm32_gpioread(GPIO_LED0))
stm32_gpiowrite(GPIO_LED0, false);
else
stm32_gpiowrite(GPIO_LED0, true);
break;
case 1:
if (stm32_gpioread(GPIO_LED1))
stm32_gpiowrite(GPIO_LED1, false);
else
stm32_gpiowrite(GPIO_LED1, true);
break;
default:
warnx("LED ID not recognized\n");
}
}
src/modules/position_estimator_mc/position_estimator_mc_params.h → src/drivers/boards/aerocore/aerocore_pwm_servo.c
Executable → Regular
+77 -29
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.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
@@ -35,35 +32,86 @@
****************************************************************************/
/*
* @file position_estimator_mc_params.h
*
* Parameters for Position Estimator
* @file aerocore_pwm_servo.c
*
* Configuration data for the stm32 pwm_servo driver.
*
* Note that these arrays must always be fully-sized.
*/
#include <systemlib/param/param.h>
#include <stdint.h>
struct position_estimator_mc_params {
float addNoise;
float sigma;
float R;
int baro; /* consider barometer */
#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
}
};
struct position_estimator_mc_param_handles {
param_t addNoise;
param_t sigma;
param_t r;
param_t baro_param_handle;
__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,
}
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct position_estimator_mc_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
src/drivers/boards/aerocore/aerocore_spi.c
+183
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
src/drivers/boards/aerocore/board_config.h
+176
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
src/drivers/boards/aerocore/module.mk
+8
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
src/drivers/boards/px4fmu-v1/board_config.h
+2
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
src/drivers/boards/px4fmu-v2/board_config.h
+2
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
src/drivers/drv_gpio.h
+9 -1
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 */
src/drivers/drv_gps.h
+4
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"
src/drivers/frsky_telemetry/frsky_data.c
+7 -10
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;
src/drivers/frsky_telemetry/frsky_telemetry.c
+1 -1
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);
src/drivers/frsky_telemetry/module.mk
+2
View File
@@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.c \
frsky_telemetry.c
MODULE_STACKSIZE = 1200
src/drivers/gps/gps.cpp
+17 -12
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);
@@ -276,14 +274,14 @@ GPS::task_main()
_report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)400e3f;
_report.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 3.0f;
_report.epv_m = 7.0f;
_report.eph_m = 0.9f;
_report.epv_m = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
@@ -421,7 +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
src/drivers/gps/module.mk
+2
View File
@@ -41,3 +41,5 @@ SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
src/drivers/gps/mtk.cpp
+8 -2
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_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
_gps_position->epv_m = _gps_position->eph_m; // 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;
src/drivers/hil/hil.cpp
+1 -1
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,
src/drivers/hmc5883/hmc5883.cpp
+6 -4
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);
}
}
}
src/drivers/hott/messages.cpp
+17 -15
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
src/drivers/l3gd20/l3gd20.cpp
+38 -18
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;
src/drivers/lsm303d/lsm303d.cpp
+1 -1
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");
src/drivers/mkblctrl/mkblctrl.cpp
+17 -21
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
{
src/drivers/mpu6000/mpu6000.cpp
-1
View File
@@ -1380,7 +1380,6 @@ MPU6000_gyro::init()
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
out:
return ret;
}
src/drivers/ms5611/ms5611.cpp
+7 -2
View File
@@ -526,6 +526,7 @@ void
MS5611::cycle()
{
int ret;
unsigned dummy;
/* collection phase? */
if (_collect_phase) {
@@ -542,6 +543,8 @@ MS5611::cycle()
} else {
//log("collection error %d", ret);
}
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
return;
@@ -573,6 +576,8 @@ MS5611::cycle()
ret = measure();
if (ret != OK) {
//log("measure error %d", ret);
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
return;
@@ -748,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);
src/drivers/ms5611/ms5611_spi.cpp
+1 -1
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) :
src/drivers/px4fmu/fmu.cpp
+251 -112
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;
@@ -120,19 +124,25 @@ private:
uint32_t _pwm_alt_rate_channels;
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_actuator_armed;
orb_advert_t _t_outputs;
int _armed_sub;
orb_advert_t _outputs_pub;
actuator_armed_s _armed;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
bool _servo_armed;
bool _pwm_on;
MixerGroup *_mixers;
actuator_controls_s _controls;
uint32_t _groups_required;
uint32_t _groups_subscribed;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
pwm_limit_t _pwm_limit;
uint16_t _failsafe_pwm[_max_actuators];
@@ -143,13 +153,13 @@ 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,
uint8_t control_index,
float &input);
void subscribe();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -197,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]);
@@ -216,15 +240,18 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_control_subs({-1}),
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(-1),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_servo_armed(false),
_pwm_on(false),
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
@@ -235,6 +262,14 @@ PX4FMU::PX4FMU() :
_max_pwm[i] = PWM_DEFAULT_MAX;
}
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
_debug_enabled = true;
}
@@ -365,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");
@@ -447,33 +496,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
}
void
PX4FMU::subscribe()
{
/* subscribe/unsubscribe to required actuator control groups */
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
void
PX4FMU::task_main()
{
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* force a reset of the update rate */
_current_update_rate = 0;
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
@@ -491,6 +550,12 @@ PX4FMU::task_main()
/* loop until killed */
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
@@ -515,20 +580,23 @@ PX4FMU::task_main()
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
orb_set_interval(_t_actuators, update_rate_in_ms);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
* 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) {
@@ -537,89 +605,101 @@ PX4FMU::task_main()
} else {
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
unsigned num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* 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) {
/*
* 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
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
poll_id++;
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* can we mix? */
if (_mixers != nullptr) {
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
unsigned num_outputs;
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
if (_armed != set_armed)
_armed = set_armed;
case MODE_4PWM:
num_outputs = 4;
break;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
case MODE_6PWM:
num_outputs = 6;
break;
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* 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) {
/*
* 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
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* publish mixed control outputs */
if (_outputs_pub < 0) {
_outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
} else {
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
}
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = _armed.armed && !_armed.lockdown;
if (_servo_armed != set_armed)
_servo_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
@@ -661,8 +741,13 @@ PX4FMU::task_main()
}
::close(_t_actuators);
::close(_t_actuator_armed);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
::close(_armed_sub);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -684,7 +769,7 @@ PX4FMU::control_callback(uintptr_t handle,
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
input = controls[control_group].control[control_index];
return 0;
}
@@ -707,6 +792,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -936,6 +1024,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_SET(7):
case PWM_SERVO_SET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@@ -963,6 +1060,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
@@ -990,12 +1096,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET_RATEGROUP(6):
case PWM_SERVO_GET_RATEGROUP(7):
#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
@@ -1041,6 +1157,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_6PWM);
break;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
case 8:
set_mode(MODE_8PWM);
break;
#endif
default:
ret = -EINVAL;
@@ -1053,6 +1174,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
@@ -1061,18 +1183,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
(uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
(uintptr_t)_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
}
break;
@@ -1083,9 +1207,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
@@ -1096,7 +1221,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
@@ -1123,10 +1252,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
unsigned count = len / 2;
uint16_t values[6];
#ifdef CONFIG_ARCH_BOARD_AEROCORE
if (count > 8) {
// we have at most 8 outputs
count = 8;
}
#else
if (count > 6) {
// we have at most 6 outputs
count = 6;
}
#endif
// allow for misaligned values
memcpy(values, buffer, count * 2);
@@ -1400,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
servo_mode = PX4FMU::MODE_6PWM;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
servo_mode = PX4FMU::MODE_8PWM;
#endif
break;
@@ -1718,7 +1857,7 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
src/drivers/px4fmu/module.mk
+2
View File
@@ -4,3 +4,5 @@
MODULE_COMMAND = fmu
SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
src/drivers/px4io/module.mk
+2
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
src/drivers/px4io/px4io.cpp
+62 -17
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;
}
@@ -683,6 +688,25 @@ PX4IO::init()
/* send command to arm system via command API */
vehicle_command_s cmd;
/* send this to itself */
param_t sys_id_param = param_find("MAV_SYS_ID");
param_t comp_id_param = param_find("MAV_COMP_ID");
int32_t sys_id;
int32_t comp_id;
if (param_get(sys_id_param, &sys_id)) {
errx(1, "PRM SYSID");
}
if (param_get(comp_id_param, &comp_id)) {
errx(1, "PRM CMPID");
}
cmd.target_system = sys_id;
cmd.target_component = comp_id;
cmd.source_system = sys_id;
cmd.source_component = comp_id;
/* request arming */
cmd.param1 = 1.0f;
cmd.param2 = 0;
@@ -692,10 +716,7 @@ PX4IO::init()
cmd.param6 = 0;
cmd.param7 = 0;
cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
// cmd.target_system = status.system_id;
// cmd.target_component = status.component_id;
// cmd.source_system = status.system_id;
// cmd.source_component = status.component_id;
/* ask to confirm command */
cmd.confirmation = 1;
@@ -773,7 +794,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);
@@ -968,13 +994,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);
}
}
}
@@ -1432,7 +1462,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
/* we don't have the status bits, so input_source has to be set elsewhere */
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];
/*
@@ -1440,8 +1470,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
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)
@@ -1453,23 +1481,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);
@@ -1477,8 +1520,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;
}
src/drivers/px4io/px4io_serial.cpp
+1 -1
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);
src/drivers/sf0x/sf0x.cpp
+2 -5
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();
src/drivers/stm32/adc/adc.cpp
+5 -1
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");
src/drivers/stm32/drv_hrt.c
+11 -3
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))
src/examples/fixedwing_control/module.mk
+2
View File
@@ -39,3 +39,5 @@ MODULE_COMMAND = ex_fixedwing_control
SRCS = main.c \
params.c
MODULE_STACKSIZE = 1200
src/examples/flow_position_estimator/flow_position_estimator_main.c
+3 -2
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);
src/examples/px4_daemon_app/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = px4_daemon_app
SRCS = px4_daemon_app.c
MODULE_STACKSIZE = 1200
src/examples/px4_daemon_app/px4_daemon_app.c
+1 -1
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);
src/examples/px4_mavlink_debug/module.mk
+3 -1
View File
@@ -37,4 +37,6 @@
MODULE_COMMAND = px4_mavlink_debug
SRCS = px4_mavlink_debug.c
SRCS = px4_mavlink_debug.c
MODULE_STACKSIZE = 2000
src/lib/version/version.h
+4
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_ */
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
-812
View File
@@ -1,812 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.cpp
*
* Kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rPressAlt(this, "R_PRESS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
memset(&ref, 0, sizeof(ref));
F.zero();
G.zero();
V.zero();
HAtt.zero();
RAtt.zero();
HPos.zero();
RPos.zero();
// initial state covariance matrix
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize rotation quaternion with a single raw sensor measurement
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
HPos(5, 8) = 1.0f;
// initialize all parameters
updateParams();
}
math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
return math::Quaternion(q0, q1, q2, q3);
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
warnx("initialized EKF attitude");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2
//&& _gps.counter_pos_valid > 10
) {
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
map_projection_init(&ref, lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
double(vN), double(vE), double(vD),
lat, lon, double(alt));
}
// prediction step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(&ref, lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = lat * M_RAD_TO_DEG;
_localPos.ref_lon = lon * M_RAD_TO_DEG;
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = q.to_dcm();
// euler update
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
// XXX position prediction using speed
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
// float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
// float sinPsi = sinf(psi);
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
if (yMag > M_PI_F) yMag -= 2*M_PI_F;
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(1, 1) = 1.0e10;
RAttAdjust(2, 2) = 1.0e10;
RAttAdjust(3, 3) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
// HAccel
HAtt(1, 1) = cosTheta;
HAtt(2, 0) = -cosPhi * cosTheta;
HAtt(2, 1) = sinPhi * sinTheta;
HAtt(3, 0) = sinPhi * cosTheta;
HAtt(3, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
warnx("numerical failure in att correction");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:"); y.print();
}
// update quaternions from euler
// angle correction
q.from_euler(phi, theta, psi);
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
// abort correction and return
warnx("numerical failure in gps correction");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(1, 1) = noiseAccelSq; // normalized direction
RAtt(2, 2) = noiseAccelSq;
RAtt(3, 3) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseGpsAlt = _rGpsAlt.get();
float noisePressAlt = _rPressAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
RPos(5, 5) = noisePressAlt * noisePressAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
-194
View File
@@ -1,194 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.hpp
*
* kalman filter navigation code
*/
#pragma once
//#define MATRIX_ASSERT
//#define VECTOR_ASSERT
#include <nuttx/config.h>
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <lib/geo/geo.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include <unistd.h>
/**
* Kalman filter navigation class
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
* Discrete-time extended Kalman filter
*/
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
// kalman filter
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
math::Matrix<9,9> P; /**< state covariance matrix */
math::Matrix<9,9> P0; /**< initial state covariance matrix */
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
// attitude
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
struct map_projection_reference_s ref; /**< local projection reference */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */
control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */
control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */
control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */
control::BlockParamFloat _magDip; /**< magnetic inclination with level */
control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParamFloat _g; /**< gravitational constant */
control::BlockParamFloat _faultPos; /**< fault detection threshold for position */
control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
};
src/modules/att_pos_estimator_ekf/kalman_main.cpp
-157
View File
@@ -1,157 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: James Goppert
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file kalman_main.cpp
* Combined attitude / position estimator.
*
* @author James Goppert
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int daemon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int kalman_demo_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int att_pos_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 30,
8192,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running\n");
exit(0);
} else {
warnx("not started\n");
exit(1);
}
}
usage("unrecognized command");
exit(1);
}
int kalman_demo_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace math;
thread_running = true;
KalmanNav nav(NULL, "KF");
while (!thread_should_exit) {
nav.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
src/modules/att_pos_estimator_ekf/params.c
-49
View File
@@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+9 -16
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);
src/modules/attitude_estimator_ekf/module.mk
+2
View File
@@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
MODULE_STACKSIZE = 1200
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+5 -4
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[])
{
src/modules/attitude_estimator_so3/module.mk
+2
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
src/modules/commander/accelerometer_calibration.cpp
+40 -26
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++) {
src/modules/commander/airspeed_calibration.cpp
+5 -1
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 */
src/modules/commander/commander.cpp
+212 -113
View File
@@ -119,9 +119,10 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
@@ -222,7 +223,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.
@@ -234,8 +235,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")) {
@@ -249,7 +251,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);
@@ -262,8 +264,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;
@@ -305,8 +308,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);
@@ -365,20 +369,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_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);
}
// 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);
return arming_res;
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)
@@ -418,14 +424,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (hil_ret == OK)
if (hil_ret == OK) {
ret = true;
}
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED)
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@@ -436,17 +444,21 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* 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 (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 {
@@ -457,8 +469,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} 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);
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
@@ -467,8 +479,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (main_res == TRANSITION_CHANGED)
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -481,19 +494,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 {
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;
@@ -521,7 +543,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
@@ -541,6 +563,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) {
@@ -584,6 +607,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:
@@ -597,6 +621,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);
@@ -630,9 +655,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";
@@ -724,7 +750,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);
@@ -743,8 +769,9 @@ int commander_thread_main(int argc, char *argv[])
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
uint64_t last_idle_time = 0;
uint64_t start_time = 0;
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
hrt_abstime last_auto_state_valid = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -773,6 +800,9 @@ int commander_thread_main(int argc, char *argv[])
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_s global_position;
memset(&global_position, 0, sizeof(global_position));
/* Init EPH and EPV */
global_position.eph = 1000.0f;
global_position.epv = 1000.0f;
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -877,6 +907,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);
@@ -917,6 +948,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");
}
@@ -934,23 +966,31 @@ 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;
}
} 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;
@@ -981,10 +1021,30 @@ 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);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
@@ -998,6 +1058,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
@@ -1067,8 +1128,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;
@@ -1153,22 +1215,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 {
@@ -1181,7 +1243,7 @@ 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.");
@@ -1190,7 +1252,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("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;
@@ -1203,7 +1265,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");
@@ -1211,24 +1273,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");
}
@@ -1241,7 +1303,8 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* MISSION switch */
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
@@ -1253,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} 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 = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
@@ -1270,34 +1333,39 @@ 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_DENIED) {
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) && (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);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
} 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);
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_DENIED) {
if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
}
@@ -1305,14 +1373,14 @@ int commander_thread_main(int argc, char *argv[])
} 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);
}
@@ -1326,8 +1394,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 */
@@ -1344,7 +1413,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;
@@ -1366,6 +1435,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
was_armed = armed.armed;
if (main_state_changed) {
@@ -1529,21 +1599,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);
@@ -1564,30 +1637,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
@@ -1602,9 +1680,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
@@ -1650,7 +1728,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;
@@ -1661,7 +1739,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;
@@ -1674,6 +1752,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;
@@ -1757,7 +1847,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:
@@ -1807,8 +1897,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) {
@@ -1823,8 +1914,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) {
@@ -1902,6 +1994,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) {
@@ -1916,10 +2009,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);
@@ -1939,11 +2034,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);
}
@@ -1959,11 +2056,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);
}
@@ -1973,8 +2072,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 */
src/modules/commander/commander_helper.cpp
+17 -5
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;
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+12 -12
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);
src/modules/commander/gyro_calibration.cpp
+10 -8
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;
src/modules/commander/mag_calibration.cpp
+36 -11
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);
src/modules/commander/module.mk
+4
View File
@@ -47,3 +47,7 @@ SRCS = commander.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/commander/px4_custom_mode.h
+3 -2
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 {
src/modules/commander/rc_calibration.cpp
+3 -3
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 */
src/modules/commander/state_machine_helper.cpp
+90 -72
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, "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 ||
@@ -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;
src/modules/dataman/dataman.c
+35 -17
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;
}
src/modules/dataman/dataman.h
+10 -10
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 */
src/modules/dataman/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp → src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+364 -106
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>
@@ -73,6 +73,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -90,20 +91,19 @@
*
* @ingroup apps
*/
extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint64_t IMUmsec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
{
return IMUmsec;
}
static void print_status();
class FixedwingEstimator
{
public:
@@ -152,6 +152,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */
@@ -177,6 +178,8 @@ private:
struct sensor_combined_s _sensor_combined;
#endif
struct map_projection_reference_s _pos_ref;
float _baro_ref; /**< barometer reference altitude */
float _baro_gps_offset; /**< offset between GPS and baro */
@@ -189,7 +192,13 @@ private:
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
bool _initialized;
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;
int _mavlink_fd;
@@ -199,6 +208,19 @@ private:
int32_t height_delay_ms;
int32_t mag_delay_ms;
int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -207,6 +229,19 @@ private:
param_t height_delay_ms;
param_t mag_delay_ms;
param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
@@ -235,7 +270,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace estimator
@@ -269,6 +304,8 @@ FixedwingEstimator::FixedwingEstimator() :
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
/* publications */
_att_pub(-1),
@@ -276,32 +313,68 @@ FixedwingEstimator::FixedwingEstimator() :
_local_pos_pub(-1),
_estimator_status_pub(-1),
_att({}),
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
_sensor_combined({}),
#endif
_baro_ref(0.0f),
_baro_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),
_baro_init(false),
_gps_initialized(false),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_mavlink_fd(-1),
_ekf(nullptr)
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
last_run = hrt_absolute_time();
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
_parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
_parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
_parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
_parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
_parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
_parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
_parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
_parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
_parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
_parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* fetch initial parameter values */
parameters_update();
@@ -315,6 +388,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
close(fd);
if (res) {
warnx("G SCALE FAIL");
}
}
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -322,6 +399,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
close(fd);
if (res) {
warnx("A SCALE FAIL");
}
}
fd = open(MAG_DEVICE_PATH, O_RDONLY);
@@ -329,6 +410,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
close(fd);
if (res) {
warnx("M SCALE FAIL");
}
}
}
@@ -366,6 +451,37 @@ FixedwingEstimator::parameters_update()
param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
if (_ekf) {
// _ekf->yawVarScale = 1.0f;
// _ekf->windVelSigma = 0.1f;
_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
_ekf->dVelBiasSigma = _parameters.abias_pnoise;
_ekf->magEarthSigma = _parameters.mage_pnoise;
_ekf->magBodySigma = _parameters.magb_pnoise;
// _ekf->gndHgtSigma = 0.02f;
_ekf->vneSigma = _parameters.velne_noise;
_ekf->vdSigma = _parameters.veld_noise;
_ekf->posNeSigma = _parameters.posne_noise;
_ekf->posDSigma = _parameters.posd_noise;
_ekf->magMeasurementSigma = _parameters.mag_noise;
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
}
return OK;
}
@@ -390,13 +506,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!");
@@ -410,6 +527,7 @@ FixedwingEstimator::task_main()
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
@@ -429,26 +547,11 @@ FixedwingEstimator::task_main()
orb_set_interval(_sensor_combined_sub, 4);
#endif
/* sets also parameters in the EKF object */
parameters_update();
/* set initial filter state */
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
/* wakeup source(s) */
struct pollfd fds[2];
@@ -464,9 +567,8 @@ FixedwingEstimator::task_main()
fds[1].events = POLLIN;
#endif
hrt_abstime start_time = hrt_absolute_time();
bool newDataGps = false;
bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
@@ -501,19 +603,52 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
bool mag_updated;
hrt_abstime last_sensor_timestamp;
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
hrt_abstime last_sensor_timestamp;
/* load local copies */
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -527,27 +662,46 @@ FixedwingEstimator::task_main()
}
last_sensor_timestamp = _gyro.timestamp;
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
if (isfinite(_gyro.x) &&
isfinite(_gyro.y) &&
isfinite(_gyro.z)) {
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
@@ -563,6 +717,8 @@ FixedwingEstimator::task_main()
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
@@ -573,23 +729,43 @@ FixedwingEstimator::task_main()
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f || deltaT < 0.000001f)
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
@@ -633,11 +809,13 @@ FixedwingEstimator::task_main()
perf_count(_perf_gps);
if (_gps.fix_type < 3) {
gps_updated = false;
newDataGps = false;
} else {
/* store time of valid GPS measurement */
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
_ekf->ResetPosition();
@@ -658,6 +836,21 @@ FixedwingEstimator::task_main()
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
newDataGps = true;
}
@@ -670,10 +863,17 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude;
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
if (!_baro_init) {
_baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
newHgtData = true;
} else {
newHgtData = false;
}
#ifndef SENSOR_COMBINED_SUB
@@ -682,6 +882,8 @@ FixedwingEstimator::task_main()
if (mag_updated) {
_mag_valid = true;
perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB
@@ -725,6 +927,8 @@ FixedwingEstimator::task_main()
*/
int check = _ekf->CheckAndBound();
const char* ekfname = "[ekf] ";
switch (check) {
case 0:
/* all ok */
@@ -733,26 +937,38 @@ FixedwingEstimator::task_main()
{
const char* str = "NaN in states, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 2:
{
const char* str = "stale IMU data, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 3:
{
const char* str = "switching dynamic / static state";
const char* str = "switching to dynamic state";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
default:
{
const char* str = "unknown reset condition";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
}
// If non-zero, we got a problem
// warn on fatal resets
if (check == 1) {
warnx("NUMERIC ERROR IN FILTER");
}
// If non-zero, we got a filter reset
if (check) {
struct ekf_status_report ekf_report;
@@ -768,7 +984,7 @@ FixedwingEstimator::task_main()
rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
// Copy all states or at least all that we can fit
int i = 0;
unsigned i = 0;
unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
@@ -784,6 +1000,23 @@ FixedwingEstimator::task_main()
} else {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
}
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
// Let the system re-initialize itself
continue;
}
@@ -791,49 +1024,70 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER
**/
// Wait long enough to ensure all sensors updated once
// XXX we rather want to check all updated
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
float initVelNED[3];
if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_global_initialized()) {
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3;
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
_ekf->InitialiseFilter(_ekf->velNED);
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_gps_offset = _baro_ref - _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
map_projection_global_reference(&_local_pos.ref_lat, &_local_pos.ref_lon);
_local_pos.ref_alt = alt;
_local_pos.ref_timestamp = map_projection_global_timestamp();
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
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 / 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_m, (double)_gps.epv_m, (double)math::degrees(declination));
_gps_initialized = true;
} else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
_local_pos.ref_alt = _baro_ref;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
}
}
@@ -867,10 +1121,10 @@ FixedwingEstimator::task_main()
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero();
_ekf->summedDelAng.zero();
_ekf->summedDelVel.zero();
dt = 0.0f;
}
@@ -921,9 +1175,9 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false;
}
if (newAdsData && _ekf->statesInitialised) {
if (newHgtData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
@@ -1012,7 +1266,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];
// 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];
@@ -1041,10 +1296,12 @@ FixedwingEstimator::task_main()
if (_local_pos.xy_global) {
double est_lat, est_lon;
map_projection_global_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon);
map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
_global_pos.eph = _gps.eph_m;
_global_pos.epv = _gps.epv_m;
}
if (_local_pos.v_xy_valid) {
@@ -1055,8 +1312,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;
@@ -1097,7 +1354,7 @@ FixedwingEstimator::start()
ASSERT(_estimator_task == -1);
/* start the task */
_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
6000,
@@ -1131,7 +1388,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", _ekf->dtIMU, dt, (int)IMUmsec);
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("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]);
@@ -1175,13 +1433,13 @@ int FixedwingEstimator::trip_nan() {
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
// KH[2][2] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #1 with NaN values");
_ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
usleep(100000);
// warnx("tripping covariance #2 with NaN values");
// KHP[5][5] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #2 with NaN values");
_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #3 with NaN values");
_ekf->P[3][3] = nan_val; // covariance matrix
@@ -1203,10 +1461,10 @@ int FixedwingEstimator::trip_nan() {
return ret;
}
int fw_att_pos_estimator_main(int argc, char *argv[])
int ekf_att_pos_estimator_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: fw_att_pos_estimator {start|stop|status}");
errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
if (!strcmp(argv[1], "start")) {
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+271
View File
@@ -0,0 +1,271 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ekf_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.0000001
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.0001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
src/modules/ekf_att_pos_estimator/estimator.cpp
+2609
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_att_pos_estimator/estimator.h → src/modules/ekf_att_pos_estimator/estimator.h
+111 -18
View File
@@ -20,7 +20,7 @@ public:
float z;
float length(void) const;
Vector3f zero(void) const;
void zero(void);
};
class Mat3f
@@ -33,6 +33,7 @@ public:
Mat3f();
void identity();
Mat3f transpose(void) const;
};
@@ -45,14 +46,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
const unsigned int n_states = 21;
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
// extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
@@ -82,6 +78,88 @@ public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
}
struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
@@ -96,6 +174,7 @@ public:
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -104,6 +183,10 @@ public:
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
@@ -115,26 +198,30 @@ public:
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS; // ratio f true to equivalent airspeed
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
@@ -148,11 +235,13 @@ public:
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
@@ -172,6 +261,10 @@ void FuseMagnetometer();
void FuseAirspeed();
void FuseRangeFinder();
void FuseOpticalFlow();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@@ -192,7 +285,7 @@ void StoreStates(uint64_t timestamp_ms);
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float statesForFusion[n_states], uint64_t msec);
int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
@@ -206,7 +299,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
@@ -218,7 +311,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3]);
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
@@ -243,7 +336,7 @@ void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
@@ -251,7 +344,7 @@ bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
src/modules/fw_att_pos_estimator/module.mk → src/modules/ekf_att_pos_estimator/module.mk
+4 -4
View File
@@ -32,11 +32,11 @@
############################################################################
#
# Main Attitude and Position Estimator for Fixed Wing Aircraft
# Main EKF Attitude and Position Estimator
#
MODULE_COMMAND = fw_att_pos_estimator
MODULE_COMMAND = ekf_att_pos_estimator
SRCS = fw_att_pos_estimator_main.cpp \
fw_att_pos_estimator_params.c \
SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator.cpp
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
-169
View File
@@ -1,169 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.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 fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_att.h"
struct fw_att_control_params {
float roll_p;
float rollrate_lim;
float pitch_p;
float pitchrate_lim;
float yawrate_lim;
float pitch_roll_compensation_p;
};
struct fw_pos_control_param_handles {
param_t roll_p;
param_t rollrate_lim;
param_t pitch_p;
param_t pitchrate_lim;
param_t yawrate_lim;
param_t pitch_roll_compensation_p;
};
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->roll_p = param_find("FW_ROLL_P");
h->rollrate_lim = param_find("FW_ROLLR_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
h->yawrate_lim = param_find("FW_YAWR_LIM");
h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_p, &(p->roll_p));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
return OK;
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_pos_control_param_handles h;
static PID_t roll_controller;
static PID_t pitch_controller;
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
counter++;
return 0;
}
src/modules/fixedwing_att_control/fixedwing_att_control_att.h
-51
View File
@@ -1,51 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.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 Fixed Wing Attitude Control */
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
#define FIXEDWING_ATT_CONTROL_ATT_H_
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp);
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
-367
View File
@@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_att_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
#include "fixedwing_att_control_att.h"
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_att_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
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 */
/* Main Thread */
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* Check if there is a new position measurement or attitude setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (att_sp_updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
printf("FW ATT CONTROL: Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* set manual setpoints if required */
if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* execute attitude control if requested */
if (control_mode.flag_control_attitude_enabled) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\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_create().
*/
int fixedwing_att_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_att_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}

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