forked from Archive/PX4-Autopilot
Merged master into indoor branch
This commit is contained in:
commit
407889ea2c
|
@ -38,3 +38,4 @@ tags
|
|||
.pydevproject
|
||||
.ropeproject
|
||||
*.orig
|
||||
Firmware.zip
|
||||
|
|
|
@ -5,5 +5,8 @@
|
|||
path = NuttX
|
||||
url = git://github.com/PX4/NuttX.git
|
||||
[submodule "uavcan"]
|
||||
path = uavcan
|
||||
path = src/lib/uavcan
|
||||
url = git://github.com/pavel-kirienko/uavcan.git
|
||||
[submodule "unittests/gtest"]
|
||||
path = unittests/gtest
|
||||
url = https://github.com/sjwilks/gtest.git
|
||||
|
|
|
@ -0,0 +1,95 @@
|
|||
# Build and autotest script for PX4 Firmware
|
||||
# http://travis-ci.org
|
||||
|
||||
language: cpp
|
||||
|
||||
before_script:
|
||||
- sudo apt-get update -q
|
||||
# Travis specific tools
|
||||
- sudo apt-get install s3cmd grep zip mailutils
|
||||
# General toolchain dependencies
|
||||
- sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
|
||||
- sudo apt-get install python-serial python-argparse
|
||||
- sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget
|
||||
- pushd .
|
||||
- cd ~
|
||||
- wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2
|
||||
- tar -jxf gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2
|
||||
- exportline="export PATH=$HOME/gcc-arm-none-eabi-4_7-2014q2/bin:\$PATH"
|
||||
- if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
|
||||
- . ~/.profile
|
||||
- popd
|
||||
|
||||
git:
|
||||
depth: 500
|
||||
|
||||
env:
|
||||
global:
|
||||
# AWS KEY: $PX4_AWS_KEY
|
||||
- secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA="
|
||||
# AWS SECRET: $PX4_AWS_SECRET
|
||||
- secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk="
|
||||
- PX4_AWS_BUCKET=px4-travis
|
||||
- PX4_EMAIL_SUBJECT="Travis CI result"
|
||||
# Email address: $PX4_EMAIL
|
||||
- secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU="
|
||||
- PX4_REPORT=report.txt
|
||||
|
||||
script:
|
||||
- arm-none-eabi-gcc --version
|
||||
- echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r'
|
||||
- make tests
|
||||
- echo -en 'travis_fold:end:script.1\\r'
|
||||
- echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r'
|
||||
- make archives
|
||||
- echo -en 'travis_fold:end:script.2\\r'
|
||||
- echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r'
|
||||
- make -j6
|
||||
- echo -en 'travis_fold:end:script.3\\r'
|
||||
- zip Firmware.zip Images/*.px4
|
||||
|
||||
# We use an encrypted env variable to ensure this only executes when artifacts are uploaded.
|
||||
after_script:
|
||||
- echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
|
||||
- git log -n1 > $PX4_REPORT
|
||||
- echo " " >> $PX4_REPORT
|
||||
- echo "Files available at:" >> $PX4_REPORT
|
||||
- echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
|
||||
- echo "Description of desired tests is available at:" >> $PX4_REPORT
|
||||
- echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
|
||||
- echo " " >> $PX4_REPORT
|
||||
- echo "Thanks for testing!" >> $PX4_REPORT
|
||||
- echo " " >> $PX4_REPORT
|
||||
- /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT"
|
||||
#- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/
|
||||
|
||||
deploy:
|
||||
provider: releases
|
||||
api_key:
|
||||
secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc=
|
||||
file: "Firmware.zip"
|
||||
skip_cleanup: true
|
||||
on:
|
||||
tags: true
|
||||
all_branches: true
|
||||
repo: PX4/Firmware
|
||||
|
||||
addons:
|
||||
artifacts:
|
||||
paths:
|
||||
- "Firmware.zip"
|
||||
key:
|
||||
secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM=
|
||||
secret:
|
||||
secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y=
|
||||
bucket: px4-travis
|
||||
region: us-east-1
|
||||
endpoint: s3-website-us-east-1.amazonaws.com
|
||||
|
||||
notifications:
|
||||
webhooks:
|
||||
urls:
|
||||
- https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba
|
||||
on_success: always # options: [always|never|change] default: always
|
||||
on_failure: always # options: [always|never|change] default: always
|
||||
on_start: false # default: false
|
18
Makefile
18
Makefile
|
@ -229,6 +229,14 @@ updatesubmodules:
|
|||
#
|
||||
testbuild:
|
||||
$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
|
||||
$(Q) (zip -r Firmware.zip $(PX4_BASE)/Images)
|
||||
|
||||
#
|
||||
# Unittest targets. Builds and runs the host-level
|
||||
# unit tests.
|
||||
.PHONY: tests
|
||||
tests:
|
||||
$(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests)
|
||||
|
||||
#
|
||||
# Cleanup targets. 'clean' should remove all built products and force
|
||||
|
@ -237,14 +245,14 @@ testbuild:
|
|||
#
|
||||
.PHONY: clean
|
||||
clean:
|
||||
$(Q) $(RMDIR) $(BUILD_DIR)*.build
|
||||
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
|
||||
$(Q) $(RMDIR) $(BUILD_DIR)*.build > /dev/null
|
||||
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4 > /dev/null
|
||||
|
||||
.PHONY: distclean
|
||||
distclean: clean
|
||||
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
|
||||
$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
|
||||
$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
|
||||
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export > /dev/null
|
||||
$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean > /dev/null
|
||||
$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) > /dev/null
|
||||
|
||||
#
|
||||
# Print some help text
|
||||
|
|
2
NuttX
2
NuttX
|
@ -1 +1 @@
|
|||
Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f
|
||||
Subproject commit 3c36467c0d5572431a09ae50013328a4693ee070
|
29
README.md
29
README.md
|
@ -1,10 +1,33 @@
|
|||
## PX4 Aerial Middleware and Flight Control Stack ##
|
||||
## PX4 Flight Control Stack and Middleware ##
|
||||
|
||||
[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware)
|
||||
|
||||
[![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
|
||||
|
||||
* Official Website: http://px4.io
|
||||
* License: BSD 3-clause (see LICENSE.md)
|
||||
* Supported airframes:
|
||||
* Multicopters
|
||||
* Fixed wing
|
||||
* [Multicopters](http://px4.io/platforms/multicopters/start)
|
||||
* [Fixed wing](http://px4.io/platforms/planes/start)
|
||||
* [VTOL](http://px4.io/platforms/vtol/start)
|
||||
* Binaries (always up-to-date from master):
|
||||
* [Downloads](https://pixhawk.org/downloads)
|
||||
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
|
||||
|
||||
### Developers ###
|
||||
|
||||
Contributing guide:
|
||||
http://px4.io/dev/contributing
|
||||
|
||||
Developer guide:
|
||||
http://px4.io/dev/
|
||||
|
||||
This repository contains code supporting these boards:
|
||||
* PX4FMUv1.x
|
||||
* PX4FMUv2.x
|
||||
* AeroCore
|
||||
|
||||
## NuttShell (NSH) ##
|
||||
|
||||
NSH usage documentation:
|
||||
http://px4.io/users/serial_connection
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO review MC_YAWRATE_I
|
||||
param set MC_ROLL_P 8.0
|
||||
|
@ -26,5 +26,5 @@ fi
|
|||
|
||||
set MIXER FMU_quad_w
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_OUT 1234
|
||||
set PWM_MIN 1200
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO tune roll/pitch separately
|
||||
param set MC_ROLL_P 7.0
|
||||
|
@ -29,7 +29,7 @@ fi
|
|||
|
||||
set MIXER FMU_quad_w
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_OUT 1234
|
||||
|
||||
set PWM_MIN 1200
|
||||
set PWM_MAX 1950
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO tune roll/pitch separately
|
||||
param set MC_ROLL_P 7.0
|
||||
|
@ -31,4 +31,4 @@ set MIXER FMU_quad_w
|
|||
set PWM_MIN 1210
|
||||
set PWM_MAX 2100
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_OUT 1234
|
||||
|
|
|
@ -0,0 +1,31 @@
|
|||
#!nsh
|
||||
#
|
||||
# Team Blacksheep Discovery Long Range Quadcopter
|
||||
#
|
||||
# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors
|
||||
#
|
||||
# Simon Wilks <simon@px4.io>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.07
|
||||
param set MC_ROLLRATE_I 0.02
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
param set MC_PITCH_P 7.0
|
||||
param set MC_PITCHRATE_P 0.1
|
||||
param set MC_PITCHRATE_I 0.05
|
||||
param set MC_PITCHRATE_D 0.003
|
||||
param set MC_YAW_P 2.8
|
||||
param set MC_YAWRATE_P 0.4
|
||||
param set MC_YAWRATE_I 0.1
|
||||
param set MC_YAWRATE_D 0.0
|
||||
fi
|
||||
|
||||
set MIXER FMU_quad_w
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_MIN 1200
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set FW_AIRSPD_MIN 12
|
||||
param set FW_AIRSPD_TRIM 25
|
||||
|
|
|
@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
set MIXER FMU_hexa_cox
|
||||
|
||||
# Need to set all 8 channels
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
|
||||
set MIXER FMU_octo_cox
|
||||
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -0,0 +1,16 @@
|
|||
#!nsh
|
||||
#
|
||||
# Generic configuration file for caipirinha VTOL version
|
||||
#
|
||||
# Roman Bapst <bapstr@ethz.ch>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.vtol_defaults
|
||||
|
||||
set MIXER FMU_caipirinha_vtol
|
||||
|
||||
set PWM_OUT 12
|
||||
set PWM_MAX 2000
|
||||
set PWM_RATE 400
|
||||
param set VT_MOT_COUNT 2
|
||||
param set VT_IDLE_PWM_MC 1080
|
|
@ -2,4 +2,4 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
set MIXER FMU_AERT
|
||||
set MIXER skywalker
|
|
@ -4,5 +4,5 @@ sh /etc/init.d/rc.fw_defaults
|
|||
|
||||
set MIXER FMU_Q
|
||||
# Provide ESC a constant 1000 us pulse while disarmed
|
||||
set PWM_OUTPUTS 4
|
||||
set PWM_OUT 4
|
||||
set PWM_DISARMED 1000
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set FW_AIRSPD_MIN 13
|
||||
param set FW_AIRSPD_TRIM 15
|
||||
|
@ -36,5 +36,5 @@ fi
|
|||
set MIXER phantom
|
||||
|
||||
# Provide ESC a constant 1000 us pulse
|
||||
set PWM_OUTPUTS 4
|
||||
set PWM_OUT 4
|
||||
set PWM_DISARMED 1000
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set FW_AIRSPD_MIN 15
|
||||
param set FW_AIRSPD_TRIM 20
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set BAT_N_CELLS 2
|
||||
param set FW_AIRSPD_MAX 15
|
||||
|
@ -44,5 +44,5 @@ fi
|
|||
|
||||
set MIXER wingwing
|
||||
# Provide ESC a constant 1000 us pulse
|
||||
set PWM_OUTPUTS 4
|
||||
set PWM_OUT 4
|
||||
set PWM_DISARMED 1000
|
||||
|
|
|
@ -7,9 +7,9 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
|
||||
|
||||
# TODO: these are the X5 default parameters, update them to the caipi
|
||||
|
||||
param set FW_AIRSPD_MIN 15
|
||||
|
|
|
@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
|
||||
set MIXER FMU_quad_x
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_OUT 1234
|
||||
|
|
|
@ -8,7 +8,7 @@ sh /etc/init.d/rc.mc_defaults
|
|||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
param set MC_ROLL_P 6.0
|
||||
|
@ -24,7 +24,7 @@ then
|
|||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAW_FF 0.8
|
||||
|
||||
|
||||
param set BAT_V_SCALING 0.00838095238
|
||||
fi
|
||||
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO REVIEW
|
||||
param set MC_ROLL_P 7.0
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO REVIEW
|
||||
param set MC_ROLL_P 7.0
|
||||
|
|
|
@ -9,7 +9,7 @@ echo "HK Micro PCB Quad"
|
|||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
|
|
|
@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
|
||||
set MIXER FMU_quad_+
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_OUT 1234
|
||||
|
|
|
@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
set MIXER FMU_hexa_x
|
||||
|
||||
# Need to set all 8 channels
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
set MIXER FMU_hexa_+
|
||||
|
||||
# Need to set all 8 channels
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
|
||||
set MIXER FMU_octo_x
|
||||
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
|
|||
|
||||
set MIXER FMU_octo_+
|
||||
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUT 12345678
|
||||
|
|
|
@ -15,6 +15,7 @@
|
|||
# 10000 .. 10999 Wide arm / H frame
|
||||
# 11000 .. 11999 Hexa Cox
|
||||
# 12000 .. 12999 Octo Cox
|
||||
# 13000 .. 13999 VTOL
|
||||
|
||||
#
|
||||
# Simulation
|
||||
|
@ -220,6 +221,11 @@ then
|
|||
sh /etc/init.d/10017_steadidrone_qu4d
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 10018 18
|
||||
then
|
||||
sh /etc/init.d/10018_tbs_endurance
|
||||
fi
|
||||
|
||||
#
|
||||
# Hexa Coaxial
|
||||
#
|
||||
|
@ -237,3 +243,13 @@ if param compare SYS_AUTOSTART 12001
|
|||
then
|
||||
sh /etc/init.d/12001_octo_cox
|
||||
fi
|
||||
|
||||
# 13000 is historically reserved for the quadshot
|
||||
|
||||
#
|
||||
# VTOL caipririnha
|
||||
#
|
||||
if param compare SYS_AUTOSTART 13001
|
||||
then
|
||||
sh /etc/init.d/13001_caipirinha_vtol
|
||||
fi
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
set VEHICLE_TYPE fw
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
#
|
||||
# Default parameters for FW
|
||||
|
@ -15,4 +15,4 @@ then
|
|||
param set FW_T_RLL2THR 15
|
||||
param set FW_T_SRATE_P 0.01
|
||||
param set FW_T_TIME_CONST 5
|
||||
fi
|
||||
fi
|
||||
|
|
|
@ -8,12 +8,11 @@ then
|
|||
#
|
||||
# Load mixer
|
||||
#
|
||||
set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
|
||||
|
||||
#Use the mixer file from the SD-card if it exists
|
||||
if [ -f $MIXERSD ]
|
||||
if [ -f /fs/microsd/etc/mixers/$MIXER.mix ]
|
||||
then
|
||||
set MIXER_FILE $MIXERSD
|
||||
set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix
|
||||
else
|
||||
set MIXER_FILE /etc/mixers/$MIXER.mix
|
||||
fi
|
||||
|
@ -32,30 +31,31 @@ then
|
|||
|
||||
if mixer load $OUTPUT_DEV $MIXER_FILE
|
||||
then
|
||||
echo "[init] Mixer loaded: $MIXER_FILE"
|
||||
echo "[i] Mixer: $MIXER_FILE"
|
||||
else
|
||||
echo "[init] Error loading mixer: $MIXER_FILE"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] Error loading mixer: $MIXER_FILE"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
|
||||
unset MIXER_FILE
|
||||
else
|
||||
if [ $MIXER != skip ]
|
||||
then
|
||||
echo "[init] Mixer not defined"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] Mixer not defined"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
|
||||
then
|
||||
if [ $PWM_OUTPUTS != none ]
|
||||
if [ $PWM_OUT != none ]
|
||||
then
|
||||
#
|
||||
# Set PWM output frequency
|
||||
#
|
||||
if [ $PWM_RATE != none ]
|
||||
then
|
||||
echo "[init] Set PWM rate: $PWM_RATE"
|
||||
pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
|
||||
pwm rate -c $PWM_OUT -r $PWM_RATE
|
||||
fi
|
||||
|
||||
#
|
||||
|
@ -63,18 +63,15 @@ then
|
|||
#
|
||||
if [ $PWM_DISARMED != none ]
|
||||
then
|
||||
echo "[init] Set PWM disarmed: $PWM_DISARMED"
|
||||
pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
|
||||
pwm disarmed -c $PWM_OUT -p $PWM_DISARMED
|
||||
fi
|
||||
if [ $PWM_MIN != none ]
|
||||
then
|
||||
echo "[init] Set PWM min: $PWM_MIN"
|
||||
pwm min -c $PWM_OUTPUTS -p $PWM_MIN
|
||||
pwm min -c $PWM_OUT -p $PWM_MIN
|
||||
fi
|
||||
if [ $PWM_MAX != none ]
|
||||
then
|
||||
echo "[init] Set PWM max: $PWM_MAX"
|
||||
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
|
||||
pwm max -c $PWM_OUT -p $PWM_MAX
|
||||
fi
|
||||
fi
|
||||
|
||||
|
|
|
@ -16,5 +16,5 @@ then
|
|||
set PX4IO_LIMIT 200
|
||||
fi
|
||||
|
||||
echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
|
||||
echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
|
||||
px4io limit $PX4IO_LIMIT
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
set VEHICLE_TYPE mc
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
|
|
|
@ -71,6 +71,10 @@ if px4flow start
|
|||
then
|
||||
fi
|
||||
|
||||
if ll40ls start
|
||||
then
|
||||
fi
|
||||
|
||||
#
|
||||
# Start sensors -> preflight_check
|
||||
#
|
||||
|
|
|
@ -10,9 +10,9 @@ then
|
|||
# First sensor publisher to initialize takes lowest instance ID
|
||||
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
|
||||
sleep 1
|
||||
echo "[init] UAVCAN started"
|
||||
echo "[i] UAVCAN started"
|
||||
else
|
||||
echo "[init] ERROR: Could not start UAVCAN"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: Could not start UAVCAN"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
|
|
@ -0,0 +1,15 @@
|
|||
#!nsh
|
||||
#
|
||||
# Standard apps for vtol:
|
||||
# att & pos estimator, att & pos control.
|
||||
#
|
||||
|
||||
attitude_estimator_ekf start
|
||||
#ekf_att_pos_estimator start
|
||||
position_estimator_inav start
|
||||
|
||||
vtol_att_control start
|
||||
mc_att_control start
|
||||
mc_pos_control start
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
|
@ -0,0 +1,39 @@
|
|||
#!nsh
|
||||
|
||||
set VEHICLE_TYPE vtol
|
||||
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
#
|
||||
#Default controller parameters for MC
|
||||
#
|
||||
param set MC_ROLL_P 6.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
param set MC_ROLLRATE_I 0.0
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
param set MC_PITCH_P 6.0
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.003
|
||||
param set MC_YAW_P 4
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAW_FF 0.3
|
||||
|
||||
#
|
||||
# Default parameters for FW
|
||||
#
|
||||
param set FW_PR_FF 0.3
|
||||
param set FW_PR_I 0.000
|
||||
param set FW_PR_IMAX 0.2
|
||||
param set FW_PR_P 0.02
|
||||
param set FW_RR_FF 0.3
|
||||
param set FW_RR_I 0.00
|
||||
param set FW_RR_IMAX 0.2
|
||||
param set FW_RR_P 0.02
|
||||
fi
|
||||
|
||||
set PWM_DISARMED 900
|
||||
set PWM_MIN 1000
|
||||
set PWM_MAX 2000
|
|
@ -1,46 +1,46 @@
|
|||
#!nsh
|
||||
#
|
||||
# PX4FMU startup script.
|
||||
#
|
||||
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
|
||||
#
|
||||
|
||||
#
|
||||
# Default to auto-start mode.
|
||||
#
|
||||
set MODE autostart
|
||||
|
||||
set RC_FILE /fs/microsd/etc/rc.txt
|
||||
set CONFIG_FILE /fs/microsd/etc/config.txt
|
||||
set EXTRAS_FILE /fs/microsd/etc/extras.txt
|
||||
set FRC /fs/microsd/etc/rc.txt
|
||||
set FCONFIG /fs/microsd/etc/config.txt
|
||||
set FEXTRAS /fs/microsd/etc/extras.txt
|
||||
|
||||
set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
|
||||
set TUNE_ERR ML<<CP4CP4CP4CP4CP4
|
||||
|
||||
#
|
||||
# Try to mount the microSD card.
|
||||
#
|
||||
echo "[init] Looking for microSD..."
|
||||
echo "[i] Looking for microSD..."
|
||||
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
||||
then
|
||||
set LOG_FILE /fs/microsd/bootlog.txt
|
||||
echo "[init] microSD mounted: /fs/microsd"
|
||||
echo "[i] microSD mounted: /fs/microsd"
|
||||
# Start playing the startup tune
|
||||
tone_alarm start
|
||||
else
|
||||
set LOG_FILE /dev/null
|
||||
echo "[init] No microSD card found"
|
||||
# Play SOS
|
||||
tone_alarm error
|
||||
fi
|
||||
|
||||
#
|
||||
# Look for an init script on the microSD card.
|
||||
# Disable autostart if the script found.
|
||||
#
|
||||
if [ -f $RC_FILE ]
|
||||
if [ -f $FRC ]
|
||||
then
|
||||
echo "[init] Executing init script: $RC_FILE"
|
||||
sh $RC_FILE
|
||||
echo "[i] Executing init script: $FRC"
|
||||
sh $FRC
|
||||
set MODE custom
|
||||
else
|
||||
echo "[init] Init script not found: $RC_FILE"
|
||||
echo "[i] Init script not found: $FRC"
|
||||
fi
|
||||
|
||||
# if this is an APM build then there will be a rc.APM script
|
||||
|
@ -49,17 +49,17 @@ if [ -f /etc/init.d/rc.APM ]
|
|||
then
|
||||
if sercon
|
||||
then
|
||||
echo "[init] USB interface connected"
|
||||
echo "[i] USB interface connected"
|
||||
fi
|
||||
|
||||
echo "[init] Running rc.APM"
|
||||
echo "[i] Running rc.APM"
|
||||
# if APM startup is successful then nsh will exit
|
||||
sh /etc/init.d/rc.APM
|
||||
fi
|
||||
|
||||
if [ $MODE == autostart ]
|
||||
then
|
||||
echo "[init] AUTOSTART mode"
|
||||
echo "[i] AUTOSTART mode"
|
||||
|
||||
#
|
||||
# Start CDC/ACM serial driver
|
||||
|
@ -117,31 +117,31 @@ then
|
|||
set VEHICLE_TYPE none
|
||||
set MIXER none
|
||||
set OUTPUT_MODE none
|
||||
set PWM_OUTPUTS none
|
||||
set PWM_OUT none
|
||||
set PWM_RATE none
|
||||
set PWM_DISARMED none
|
||||
set PWM_MIN none
|
||||
set PWM_MAX none
|
||||
set MKBLCTRL_MODE none
|
||||
set MK_MODE none
|
||||
set FMU_MODE pwm
|
||||
set MAVLINK_FLAGS default
|
||||
set MAVLINK_F default
|
||||
set EXIT_ON_END no
|
||||
set MAV_TYPE none
|
||||
set LOAD_DEFAULT_APPS yes
|
||||
set LOAD_DAPPS yes
|
||||
set GPS yes
|
||||
set GPS_FAKE no
|
||||
set FAILSAFE none
|
||||
|
||||
#
|
||||
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
|
||||
# Set AUTOCNF flag to use it in AUTOSTART scripts
|
||||
#
|
||||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# Wipe out params
|
||||
param reset_nostart
|
||||
set DO_AUTOCONFIG yes
|
||||
set AUTOCNF yes
|
||||
else
|
||||
set DO_AUTOCONFIG no
|
||||
set AUTOCNF no
|
||||
fi
|
||||
|
||||
#
|
||||
|
@ -159,7 +159,7 @@ then
|
|||
#
|
||||
if param compare SYS_AUTOSTART 0
|
||||
then
|
||||
echo "[init] No autostart"
|
||||
echo "[i] No autostart"
|
||||
else
|
||||
sh /etc/init.d/rc.autostart
|
||||
fi
|
||||
|
@ -167,18 +167,19 @@ then
|
|||
#
|
||||
# Override parameters from user configuration file
|
||||
#
|
||||
if [ -f $CONFIG_FILE ]
|
||||
if [ -f $FCONFIG ]
|
||||
then
|
||||
echo "[init] Config: $CONFIG_FILE"
|
||||
sh $CONFIG_FILE
|
||||
echo "[i] Config: $FCONFIG"
|
||||
sh $FCONFIG
|
||||
else
|
||||
echo "[init] Config not found: $CONFIG_FILE"
|
||||
echo "[i] Config not found: $FCONFIG"
|
||||
fi
|
||||
unset FCONFIG
|
||||
|
||||
#
|
||||
# If autoconfig parameter was set, reset it and save parameters
|
||||
#
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
|
@ -219,18 +220,18 @@ then
|
|||
set IO_PRESENT yes
|
||||
else
|
||||
echo "PX4IO update failed" >> $LOG_FILE
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
else
|
||||
echo "PX4IO update failed" >> $LOG_FILE
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $IO_PRESENT == no ]
|
||||
then
|
||||
echo "[init] ERROR: PX4IO not found"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: PX4IO not found"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
@ -251,7 +252,7 @@ then
|
|||
then
|
||||
# Need IO for output but it not present, disable output
|
||||
set OUTPUT_MODE none
|
||||
echo "[init] ERROR: PX4IO not found, disabling output"
|
||||
echo "[i] ERROR: PX4IO not found, disabling output"
|
||||
|
||||
# Avoid using ttyS0 for MAVLink on FMUv1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
|
@ -294,33 +295,31 @@ then
|
|||
then
|
||||
if param compare UAVCAN_ENABLE 0
|
||||
then
|
||||
echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
|
||||
echo "[i] OVERRIDING UAVCAN_ENABLE = 1"
|
||||
param set UAVCAN_ENABLE 1
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
|
||||
then
|
||||
echo "[init] Use PX4IO PWM as primary output"
|
||||
if px4io start
|
||||
then
|
||||
echo "[init] PX4IO started"
|
||||
echo "[i] PX4IO started"
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
echo "[init] ERROR: PX4IO start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: PX4IO start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
|
||||
then
|
||||
echo "[init] Use FMU as primary output"
|
||||
if fmu mode_$FMU_MODE
|
||||
then
|
||||
echo "[init] FMU mode_$FMU_MODE started"
|
||||
echo "[i] FMU mode_$FMU_MODE started"
|
||||
else
|
||||
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
|
@ -338,36 +337,34 @@ then
|
|||
|
||||
if [ $OUTPUT_MODE == mkblctrl ]
|
||||
then
|
||||
echo "[init] Use MKBLCTRL as primary output"
|
||||
set MKBLCTRL_ARG ""
|
||||
if [ $MKBLCTRL_MODE == x ]
|
||||
if [ $MK_MODE == x ]
|
||||
then
|
||||
set MKBLCTRL_ARG "-mkmode x"
|
||||
fi
|
||||
if [ $MKBLCTRL_MODE == + ]
|
||||
if [ $MK_MODE == + ]
|
||||
then
|
||||
set MKBLCTRL_ARG "-mkmode +"
|
||||
fi
|
||||
|
||||
if mkblctrl $MKBLCTRL_ARG
|
||||
then
|
||||
echo "[init] MKBLCTRL started"
|
||||
echo "[i] MK started"
|
||||
else
|
||||
echo "[init] ERROR: MKBLCTRL start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: MK start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE == hil ]
|
||||
then
|
||||
echo "[init] Use HIL as primary output"
|
||||
if hil mode_port2_pwm8
|
||||
then
|
||||
echo "[init] HIL output started"
|
||||
echo "[i] HIL output started"
|
||||
else
|
||||
echo "[init] ERROR: HIL output start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: HIL output start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
@ -380,11 +377,11 @@ then
|
|||
then
|
||||
if px4io start
|
||||
then
|
||||
echo "[init] PX4IO started"
|
||||
echo "[i] PX4IO started"
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
echo "[init] ERROR: PX4IO start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: PX4IO start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
else
|
||||
|
@ -392,10 +389,10 @@ then
|
|||
then
|
||||
if fmu mode_$FMU_MODE
|
||||
then
|
||||
echo "[init] FMU mode_$FMU_MODE started"
|
||||
echo "[i] FMU mode_$FMU_MODE started"
|
||||
else
|
||||
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
|
@ -413,23 +410,24 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
if [ $MAVLINK_FLAGS == default ]
|
||||
if [ $MAVLINK_F == default ]
|
||||
then
|
||||
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
|
||||
if [ $TTYS1_BUSY == yes ]
|
||||
then
|
||||
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
|
||||
set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
|
||||
set MAVLINK_F "-r 1000 -d /dev/ttyS0"
|
||||
|
||||
# Exit from nsh to free port for mavlink
|
||||
set EXIT_ON_END yes
|
||||
else
|
||||
# Start MAVLink on default port: ttyS1
|
||||
set MAVLINK_FLAGS "-r 1000"
|
||||
set MAVLINK_F "-r 1000"
|
||||
fi
|
||||
fi
|
||||
|
||||
mavlink start $MAVLINK_FLAGS
|
||||
mavlink start $MAVLINK_F
|
||||
unset MAVLINK_F
|
||||
|
||||
#
|
||||
# MAVLink onboard / TELEM2
|
||||
|
@ -451,15 +449,16 @@ then
|
|||
|
||||
if [ $GPS == yes ]
|
||||
then
|
||||
echo "[init] Start GPS"
|
||||
echo "[i] Start GPS"
|
||||
if [ $GPS_FAKE == yes ]
|
||||
then
|
||||
echo "[init] Faking GPS"
|
||||
echo "[i] Faking GPS"
|
||||
gps start -f
|
||||
else
|
||||
gps start
|
||||
fi
|
||||
fi
|
||||
unset GPS_FAKE
|
||||
|
||||
#
|
||||
# Start up ARDrone Motor interface
|
||||
|
@ -474,7 +473,7 @@ then
|
|||
#
|
||||
if [ $VEHICLE_TYPE == fw ]
|
||||
then
|
||||
echo "[init] Vehicle type: FIXED WING"
|
||||
echo "[i] FIXED WING"
|
||||
|
||||
if [ $MIXER == none ]
|
||||
then
|
||||
|
@ -494,7 +493,7 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard fixedwing apps
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
if [ $LOAD_DAPPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.fw_apps
|
||||
fi
|
||||
|
@ -505,11 +504,11 @@ then
|
|||
#
|
||||
if [ $VEHICLE_TYPE == mc ]
|
||||
then
|
||||
echo "[init] Vehicle type: MULTICOPTER"
|
||||
echo "[i] MULTICOPTER"
|
||||
|
||||
if [ $MIXER == none ]
|
||||
then
|
||||
echo "Default mixer for multicopter not defined"
|
||||
echo "Mixer undefined"
|
||||
fi
|
||||
|
||||
if [ $MAV_TYPE == none ]
|
||||
|
@ -553,12 +552,51 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard multicopter apps
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
if [ $LOAD_DAPPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.mc_apps
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# VTOL setup
|
||||
#
|
||||
if [ $VEHICLE_TYPE == vtol ]
|
||||
then
|
||||
echo "[init] Vehicle type: VTOL"
|
||||
|
||||
if [ $MIXER == none ]
|
||||
then
|
||||
echo "Default mixer for vtol not defined"
|
||||
fi
|
||||
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
# Use mixer to detect vehicle type
|
||||
if [ $MIXER == FMU_caipirinha_vtol ]
|
||||
then
|
||||
set MAV_TYPE 19
|
||||
fi
|
||||
fi
|
||||
|
||||
# Still no MAV_TYPE found
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
echo "Unknown MAV_TYPE"
|
||||
else
|
||||
param set MAV_TYPE $MAV_TYPE
|
||||
fi
|
||||
|
||||
# Load mixer and configure outputs
|
||||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard vtol apps
|
||||
if [ $LOAD_DAPPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.vtol_apps
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the navigator
|
||||
#
|
||||
|
@ -569,24 +607,38 @@ then
|
|||
#
|
||||
if [ $VEHICLE_TYPE == none ]
|
||||
then
|
||||
echo "[init] Vehicle type: No autostart ID found"
|
||||
echo "[i] No autostart ID found"
|
||||
|
||||
fi
|
||||
|
||||
# Start any custom addons
|
||||
if [ -f $EXTRAS_FILE ]
|
||||
if [ -f $FEXTRAS ]
|
||||
then
|
||||
echo "[init] Starting addons script: $EXTRAS_FILE"
|
||||
sh $EXTRAS_FILE
|
||||
echo "[i] Addons script: $FEXTRAS"
|
||||
sh $FEXTRAS
|
||||
else
|
||||
echo "[init] No addons script: $EXTRAS_FILE"
|
||||
echo "[i] No addons script: $FEXTRAS"
|
||||
fi
|
||||
unset FEXTRAS
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
echo "[init] Exit from nsh"
|
||||
echo "Exit from nsh"
|
||||
exit
|
||||
fi
|
||||
unset EXIT_ON_END
|
||||
|
||||
# Run no SD alarm last
|
||||
if [ $LOG_FILE == /dev/null ]
|
||||
then
|
||||
echo "[i] No microSD card found"
|
||||
# Play SOS
|
||||
tone_alarm error
|
||||
fi
|
||||
|
||||
# End of autostart
|
||||
fi
|
||||
|
||||
# There is no further processing, so we can free some RAM
|
||||
# XXX potentially unset all script variables.
|
||||
unset TUNE_ERR
|
||||
|
|
|
@ -0,0 +1,16 @@
|
|||
#!nsh
|
||||
# Caipirinha vtol mixer for PX4FMU
|
||||
#
|
||||
#===========================
|
||||
R: 2- 10000 10000 10000 0
|
||||
|
||||
#mixer for the elevons
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 1 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 1 -10000 -10000 0 -10000 10000
|
|
@ -0,0 +1,64 @@
|
|||
Mixer for Skywalker Airframe
|
||||
==================================================
|
||||
|
||||
This file defines mixers suitable for controlling a fixed wing aircraft with
|
||||
aileron, rudder, elevator and throttle controls using PX4FMU. The configuration
|
||||
assumes the aileron servo(s) are connected to PX4FMU servo output 0, the
|
||||
elevator to output 1, the rudder to output 2 and the throttle to output 3.
|
||||
|
||||
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||
(roll), 1 (pitch) and 3 (thrust).
|
||||
|
||||
Aileron mixer
|
||||
-------------
|
||||
Two scalers total (output, roll).
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
depending on the actual configuration it may be necessary to reverse the scaling
|
||||
factors (to reverse the servo movement) and adjust the offset, scaling and
|
||||
endpoints to suit.
|
||||
|
||||
As there is only one output, if using two servos adjustments to compensate for
|
||||
differences between the servos must be made mechanically. To obtain the correct
|
||||
motion using a Y cable, the servos can be positioned reversed from one another.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 -10000 -10000 0 -10000 10000
|
||||
|
||||
Elevator mixer
|
||||
------------
|
||||
Two scalers total (output, roll).
|
||||
|
||||
This mixer assumes that the elevator servo is set up correctly mechanically;
|
||||
depending on the actual configuration it may be necessary to reverse the scaling
|
||||
factors (to reverse the servo movement) and adjust the offset, scaling and
|
||||
endpoints to suit.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 1 10000 10000 0 -10000 10000
|
||||
|
||||
Rudder mixer
|
||||
------------
|
||||
Two scalers total (output, yaw).
|
||||
|
||||
This mixer assumes that the rudder servo is set up correctly mechanically;
|
||||
depending on the actual configuration it may be necessary to reverse the scaling
|
||||
factors (to reverse the servo movement) and adjust the offset, scaling and
|
||||
endpoints to suit.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 2 10000 10000 0 -10000 10000
|
||||
|
||||
Motor speed mixer
|
||||
-----------------
|
||||
Two scalers total (output, thrust).
|
||||
|
||||
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
|
||||
range. Inputs below zero are treated as zero.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
|
@ -3,11 +3,11 @@
|
|||
# Flight startup script for PX4FMU standalone configuration.
|
||||
#
|
||||
|
||||
echo "[init] doing standalone PX4FMU startup..."
|
||||
echo "[i] doing standalone PX4FMU startup..."
|
||||
|
||||
#
|
||||
# Start the ORB
|
||||
#
|
||||
uorb start
|
||||
|
||||
echo "[init] startup done"
|
||||
echo "[i] startup done"
|
||||
|
|
|
@ -6,7 +6,7 @@ uorb start
|
|||
|
||||
if sercon
|
||||
then
|
||||
echo "[init] USB interface connected"
|
||||
echo "[i] USB interface connected"
|
||||
|
||||
# Try to get an USB console
|
||||
nshterm /dev/ttyACM0 &
|
||||
|
@ -15,14 +15,14 @@ fi
|
|||
#
|
||||
# Try to mount the microSD card.
|
||||
#
|
||||
echo "[init] looking for microSD..."
|
||||
echo "[i] looking for microSD..."
|
||||
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
||||
then
|
||||
echo "[init] card mounted at /fs/microsd"
|
||||
echo "[i] card mounted at /fs/microsd"
|
||||
# Start playing the startup tune
|
||||
tone_alarm start
|
||||
else
|
||||
echo "[init] no microSD card found"
|
||||
echo "[i] no microSD card found"
|
||||
# Play SOS
|
||||
tone_alarm error
|
||||
fi
|
||||
|
@ -104,4 +104,4 @@ then
|
|||
else
|
||||
echo
|
||||
echo "Some Unit Tests FAILED:${unit_test_failure_list}"
|
||||
fi
|
||||
fi
|
||||
|
|
|
@ -103,7 +103,7 @@ class SourceParser(object):
|
|||
Returns list of supported file extensions that can be parsed by this
|
||||
parser.
|
||||
"""
|
||||
return ["cpp", "c"]
|
||||
return [".cpp", ".c"]
|
||||
|
||||
def Parse(self, contents):
|
||||
"""
|
||||
|
|
|
@ -26,5 +26,10 @@ class SourceScanner(object):
|
|||
parser.Parse method.
|
||||
"""
|
||||
with codecs.open(path, 'r', 'utf-8') as f:
|
||||
contents = f.read()
|
||||
try:
|
||||
contents = f.read()
|
||||
except:
|
||||
contents = ''
|
||||
print('Failed reading file: %s, skipping content.' % path)
|
||||
pass
|
||||
parser.Parse(contents)
|
||||
|
|
|
@ -1,54 +0,0 @@
|
|||
|
||||
CC=g++
|
||||
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
|
||||
-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
|
||||
|
||||
all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
|
||||
|
||||
MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
|
||||
../../src/systemcmds/tests/test_conv.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_simple.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_multirotor.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_group.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_load.c \
|
||||
../../src/modules/systemlib/pwm_limit/pwm_limit.c \
|
||||
hrt.cpp \
|
||||
mixer_test.cpp
|
||||
|
||||
SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
|
||||
hrt.cpp \
|
||||
sbus2_test.cpp
|
||||
|
||||
ST24_FILES=../../src/lib/rc/st24.c \
|
||||
hrt.cpp \
|
||||
st24_test.cpp
|
||||
|
||||
SF0X_FILES= \
|
||||
hrt.cpp \
|
||||
sf0x_test.cpp \
|
||||
../../src/drivers/sf0x/sf0x_parser.cpp
|
||||
|
||||
AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
|
||||
hrt.cpp \
|
||||
autodeclination_test.cpp
|
||||
|
||||
mixer_test: $(MIXER_FILES)
|
||||
$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
|
||||
|
||||
sbus2_test: $(SBUS2_FILES)
|
||||
$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
|
||||
|
||||
sf0x_test: $(SF0X_FILES)
|
||||
$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
|
||||
|
||||
autodeclination_test: $(SBUS2_FILES)
|
||||
$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
|
||||
|
||||
st24_test: $(ST24_FILES)
|
||||
$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
|
||||
|
||||
.PHONY: clean
|
||||
|
||||
clean:
|
||||
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
|
|
@ -1,28 +0,0 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4iofirmware/px4io.h>
|
||||
#include "../../src/systemcmds/tests/tests.h"
|
||||
#include <geo/geo.h>
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
warnx("autodeclination test started");
|
||||
|
||||
if (argc < 3)
|
||||
errx(1, "Need lat/lon!");
|
||||
|
||||
char* p_end;
|
||||
|
||||
float lat = strtod(argv[1], &p_end);
|
||||
float lon = strtod(argv[2], &p_end);
|
||||
|
||||
float declination = get_mag_declination(lat, lon);
|
||||
|
||||
printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
|
||||
|
||||
}
|
|
@ -24,6 +24,8 @@ MODULES += drivers/l3gd20
|
|||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
#MODULES += drivers/ll40ls
|
||||
MODULES += drivers/trone
|
||||
#MODULES += drivers/mb12xx
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
|
|
|
@ -27,13 +27,14 @@ MODULES += drivers/l3gd20
|
|||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/sf0x
|
||||
# MODULES += drivers/sf0x
|
||||
MODULES += drivers/ll40ls
|
||||
# MODULES += drivers/trone
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
# MODULES += drivers/blinkm
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
|
@ -85,6 +86,7 @@ MODULES += modules/fw_pos_control_l1
|
|||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
MODULES += modules/vtol_att_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
|
|
|
@ -50,16 +50,25 @@ MODULES += lib/mathlib/math/filter
|
|||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Modules to test-build, but not useful for test environment
|
||||
# Example modules to test-build
|
||||
#
|
||||
MODULES += examples/flow_position_estimator
|
||||
MODULES += examples/fixedwing_control
|
||||
MODULES += examples/hwtest
|
||||
MODULES += examples/matlab_csv_serial
|
||||
MODULES += examples/px4_daemon_app
|
||||
MODULES += examples/px4_mavlink_debug
|
||||
MODULES += examples/px4_simple_app
|
||||
|
||||
#
|
||||
# Drivers / modules to test build, but not useful for test environment
|
||||
#
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += drivers/pca8574
|
||||
MODULES += examples/flow_position_estimator
|
||||
|
||||
#
|
||||
# Libraries
|
||||
# Tests
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
|
||||
MODULES += modules/unit_test
|
||||
MODULES += modules/mavlink/mavlink_tests
|
||||
|
|
|
@ -0,0 +1,10 @@
|
|||
# JTAG for the STM32F4x chip used on the Gumstix AeroCore is available on
|
||||
# the first interface of a Quad FTDI chip. nTRST is bit 4.
|
||||
interface ftdi
|
||||
ftdi_vid_pid 0x0403 0x6011
|
||||
|
||||
ftdi_layout_init 0x0000 0x001b
|
||||
ftdi_layout_signal nTRST -data 0x0010
|
||||
|
||||
source [find target/stm32f4x.cfg]
|
||||
reset_config trst_only
|
|
@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
|
|||
#
|
||||
ARCHWARNINGS = -Wall \
|
||||
-Wextra \
|
||||
-Werror \
|
||||
-Wdouble-promotion \
|
||||
-Wshadow \
|
||||
-Wfloat-equal \
|
||||
|
|
|
@ -30,6 +30,11 @@ upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER)
|
|||
upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER)
|
||||
$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
|
||||
|
||||
upload-serial-aerocore:
|
||||
openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit'
|
||||
|
||||
|
||||
|
||||
#
|
||||
# JTAG firmware uploading with OpenOCD
|
||||
#
|
||||
|
|
|
@ -139,8 +139,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
|
|||
-Wold-style-declaration \
|
||||
-Wmissing-parameter-type \
|
||||
-Wmissing-prototypes \
|
||||
-Wnested-externs \
|
||||
-Wunsuffixed-float-constants
|
||||
-Wnested-externs
|
||||
ARCHWARNINGSXX = $(ARCHWARNINGS) \
|
||||
-Wno-psabi
|
||||
ARCHDEFINES =
|
||||
|
|
|
@ -139,8 +139,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
|
|||
-Wold-style-declaration \
|
||||
-Wmissing-parameter-type \
|
||||
-Wmissing-prototypes \
|
||||
-Wnested-externs \
|
||||
-Wunsuffixed-float-constants
|
||||
-Wnested-externs
|
||||
ARCHWARNINGSXX = $(ARCHWARNINGS) \
|
||||
-Wno-psabi
|
||||
ARCHDEFINES =
|
||||
|
|
|
@ -128,8 +128,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
|
|||
-Wold-style-declaration \
|
||||
-Wmissing-parameter-type \
|
||||
-Wmissing-prototypes \
|
||||
-Wnested-externs \
|
||||
-Wunsuffixed-float-constants
|
||||
-Wnested-externs
|
||||
ARCHWARNINGSXX = $(ARCHWARNINGS)
|
||||
ARCHDEFINES =
|
||||
ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
|
||||
|
|
|
@ -128,8 +128,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
|
|||
-Wold-style-declaration \
|
||||
-Wmissing-parameter-type \
|
||||
-Wmissing-prototypes \
|
||||
-Wnested-externs \
|
||||
-Wunsuffixed-float-constants
|
||||
-Wnested-externs
|
||||
ARCHWARNINGSXX = $(ARCHWARNINGS)
|
||||
ARCHDEFINES =
|
||||
ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
|
||||
|
|
|
@ -159,13 +159,15 @@ out:
|
|||
int
|
||||
Airspeed::probe()
|
||||
{
|
||||
/* on initial power up the device needs more than one retry
|
||||
for detection. Once it is running then retries aren't
|
||||
needed
|
||||
/* on initial power up the device may need more than one retry
|
||||
for detection. Once it is running the number of retries can
|
||||
be reduced
|
||||
*/
|
||||
_retries = 4;
|
||||
int ret = measure();
|
||||
_retries = 0;
|
||||
|
||||
// drop back to 2 retries once initialised
|
||||
_retries = 2;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_MAX - 15,
|
||||
1100,
|
||||
ardrone_interface_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -45,6 +45,7 @@
|
|||
#include "board_config.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
/*
|
||||
* Ideally we'd be able to get these from up_internal.h,
|
||||
|
@ -54,7 +55,7 @@
|
|||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init();
|
||||
extern void led_init(void);
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
extern void led_toggle(int led);
|
||||
|
|
|
@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_DEFAULT,
|
||||
2000,
|
||||
frsky_telemetry_thread_main,
|
||||
(const char **)argv);
|
||||
(char * const *)argv);
|
||||
|
||||
while (!thread_running) {
|
||||
usleep(200);
|
||||
|
|
|
@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
|
|||
void
|
||||
GPS::task_main()
|
||||
{
|
||||
log("starting");
|
||||
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR);
|
||||
|
|
|
@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr;
|
|||
void start(int bus, enum Rotation rotation);
|
||||
void test(int bus);
|
||||
void reset(int bus);
|
||||
void info(int bus);
|
||||
int info(int bus);
|
||||
int calibrate(int bus);
|
||||
void usage();
|
||||
|
||||
|
@ -1595,17 +1595,23 @@ reset(int bus)
|
|||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
int
|
||||
info(int bus)
|
||||
{
|
||||
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver not running");
|
||||
int ret = 1;
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
|
||||
if (g_dev == nullptr) {
|
||||
warnx("not running on bus %d", bus);
|
||||
} else {
|
||||
|
||||
exit(0);
|
||||
warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
|
||||
|
||||
g_dev->print_info();
|
||||
ret = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[])
|
|||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
|
||||
hmc5883::info(bus);
|
||||
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
|
||||
if (bus == -1) {
|
||||
int ret = 0;
|
||||
if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
|
||||
ret = 1;
|
||||
}
|
||||
exit(ret);
|
||||
} else {
|
||||
exit(hmc5883::info(bus));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Autocalibrate the scaling
|
||||
|
|
|
@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
hott_sensors_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
hott_telemetry_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -89,7 +89,7 @@
|
|||
|
||||
/* Device limits */
|
||||
#define LL40LS_MIN_DISTANCE (0.00f)
|
||||
#define LL40LS_MAX_DISTANCE (14.00f)
|
||||
#define LL40LS_MAX_DISTANCE (60.00f)
|
||||
|
||||
#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */
|
||||
|
||||
|
@ -233,11 +233,11 @@ LL40LS::~LL40LS()
|
|||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
|
||||
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
|
||||
}
|
||||
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
|
@ -263,7 +263,7 @@ LL40LS::init()
|
|||
|
||||
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
/* get a publish handle on the range finder topic */
|
||||
struct range_finder_report rf_report;
|
||||
measure();
|
||||
|
@ -314,9 +314,9 @@ LL40LS::probe()
|
|||
goto ok;
|
||||
}
|
||||
|
||||
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
|
||||
(unsigned)who_am_i,
|
||||
LL40LS_WHO_AM_I_REG_VAL,
|
||||
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
|
||||
(unsigned)who_am_i,
|
||||
LL40LS_WHO_AM_I_REG_VAL,
|
||||
(unsigned)val);
|
||||
}
|
||||
|
||||
|
@ -581,6 +581,8 @@ LL40LS::collect()
|
|||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.minimum_distance = get_minimum_distance();
|
||||
report.maximum_distance = get_maximum_distance();
|
||||
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
|
||||
report.valid = 1;
|
||||
}
|
||||
|
@ -704,7 +706,7 @@ LL40LS::print_info()
|
|||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
_reports->print_info("report queue");
|
||||
printf("distance: %ucm (0x%04x)\n",
|
||||
printf("distance: %ucm (0x%04x)\n",
|
||||
(unsigned)_last_distance, (unsigned)_last_distance);
|
||||
}
|
||||
|
||||
|
@ -969,8 +971,8 @@ ll40ls_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
|
|
|
@ -520,6 +520,8 @@ MB12XX::collect()
|
|||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.minimum_distance = get_minimum_distance();
|
||||
report.maximum_distance = get_maximum_distance();
|
||||
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
|
||||
|
||||
/* publish it, if we are the primary */
|
||||
|
|
|
@ -194,6 +194,8 @@ public:
|
|||
*/
|
||||
void print_info();
|
||||
|
||||
void print_registers();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
|
@ -1414,6 +1416,21 @@ MPU6000::print_info()
|
|||
_gyro_reports->print_info("gyro queue");
|
||||
}
|
||||
|
||||
void
|
||||
MPU6000::print_registers()
|
||||
{
|
||||
printf("MPU6000 registers\n");
|
||||
for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
|
||||
uint8_t v = read_reg(reg);
|
||||
printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
|
||||
if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
|
||||
CDev("MPU6000_gyro", path),
|
||||
_parent(parent),
|
||||
|
@ -1479,6 +1496,7 @@ void start(bool, enum Rotation);
|
|||
void test(bool);
|
||||
void reset(bool);
|
||||
void info(bool);
|
||||
void regdump(bool);
|
||||
void usage();
|
||||
|
||||
/**
|
||||
|
@ -1654,10 +1672,26 @@ info(bool external_bus)
|
|||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Dump the register information
|
||||
*/
|
||||
void
|
||||
regdump(bool external_bus)
|
||||
{
|
||||
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
|
||||
if (*g_dev_ptr == nullptr)
|
||||
errx(1, "driver not running");
|
||||
|
||||
printf("regdump @ %p\n", *g_dev_ptr);
|
||||
(*g_dev_ptr)->print_registers();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
usage()
|
||||
{
|
||||
warnx("missing command: try 'start', 'info', 'test', 'reset'");
|
||||
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
|
||||
warnx("options:");
|
||||
warnx(" -X (external bus)");
|
||||
warnx(" -R rotation");
|
||||
|
@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[])
|
|||
if (!strcmp(verb, "info"))
|
||||
mpu6000::info(external_bus);
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
/*
|
||||
* Print register information.
|
||||
*/
|
||||
if (!strcmp(verb, "regdump"))
|
||||
mpu6000::regdump(external_bus);
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
|
||||
}
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow
|
|||
SRCS = px4flow.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
EXTRACXXFLAGS = -Wno-attributes
|
||||
|
|
|
@ -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
|
||||
|
@ -73,15 +73,13 @@
|
|||
#include <board_config.h>
|
||||
|
||||
/* Configuration Constants */
|
||||
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
|
||||
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
|
||||
//range 0x42 - 0x49
|
||||
|
||||
/* PX4FLOW Registers addresses */
|
||||
#define PX4FLOW_REG 0x00 /* Measure Register */
|
||||
|
||||
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
|
||||
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
|
||||
|
||||
#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
|
@ -92,28 +90,42 @@ static const int ERROR = -1;
|
|||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
//struct i2c_frame
|
||||
//{
|
||||
// uint16_t frame_count;
|
||||
// int16_t pixel_flow_x_sum;
|
||||
// int16_t pixel_flow_y_sum;
|
||||
// int16_t flow_comp_m_x;
|
||||
// int16_t flow_comp_m_y;
|
||||
// int16_t qual;
|
||||
// int16_t gyro_x_rate;
|
||||
// int16_t gyro_y_rate;
|
||||
// int16_t gyro_z_rate;
|
||||
// uint8_t gyro_range;
|
||||
// uint8_t sonar_timestamp;
|
||||
// int16_t ground_distance;
|
||||
//};
|
||||
//
|
||||
//struct i2c_frame f;
|
||||
struct i2c_frame {
|
||||
uint16_t frame_count;
|
||||
int16_t pixel_flow_x_sum;
|
||||
int16_t pixel_flow_y_sum;
|
||||
int16_t flow_comp_m_x;
|
||||
int16_t flow_comp_m_y;
|
||||
int16_t qual;
|
||||
int16_t gyro_x_rate;
|
||||
int16_t gyro_y_rate;
|
||||
int16_t gyro_z_rate;
|
||||
uint8_t gyro_range;
|
||||
uint8_t sonar_timestamp;
|
||||
int16_t ground_distance;
|
||||
};
|
||||
struct i2c_frame f;
|
||||
|
||||
class PX4FLOW : public device::I2C
|
||||
struct i2c_integral_frame {
|
||||
uint16_t frame_count_since_last_readout;
|
||||
int16_t pixel_flow_x_integral;
|
||||
int16_t pixel_flow_y_integral;
|
||||
int16_t gyro_x_rate_integral;
|
||||
int16_t gyro_y_rate_integral;
|
||||
int16_t gyro_z_rate_integral;
|
||||
uint32_t integration_timespan;
|
||||
uint32_t time_since_last_sonar_update;
|
||||
uint16_t ground_distance;
|
||||
int16_t gyro_temperature;
|
||||
uint8_t qual;
|
||||
} __attribute__((packed));
|
||||
struct i2c_integral_frame f_integral;
|
||||
|
||||
|
||||
class PX4FLOW: public device::I2C
|
||||
{
|
||||
public:
|
||||
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
|
||||
PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
|
||||
virtual ~PX4FLOW();
|
||||
|
||||
virtual int init();
|
||||
|
@ -122,8 +134,8 @@ public:
|
|||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
|
@ -144,42 +156,41 @@ private:
|
|||
perf_counter_t _buffer_overflows;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void cycle();
|
||||
int measure();
|
||||
int collect();
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
};
|
||||
|
||||
|
@ -189,7 +200,7 @@ private:
|
|||
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
|
||||
|
||||
PX4FLOW::PX4FLOW(int bus, int address) :
|
||||
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
|
||||
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
|
@ -228,21 +239,12 @@ PX4FLOW::init()
|
|||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
|
||||
_reports = new RingBuffer(2, sizeof(optical_flow_s));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* get a publish handle on the px4flow topic */
|
||||
struct optical_flow_s zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
|
||||
|
||||
if (_px4flow_topic < 0) {
|
||||
warnx("failed to create px4flow object. Did you start uOrb?");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
|
@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
|
|||
break;
|
||||
}
|
||||
|
||||
/* wait for it to complete */
|
||||
usleep(PX4FLOW_CONVERSION_INTERVAL);
|
||||
|
||||
/* run the collection phase */
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
|
@ -442,6 +441,7 @@ PX4FLOW::measure()
|
|||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
debug("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -453,14 +453,20 @@ PX4FLOW::measure()
|
|||
int
|
||||
PX4FLOW::collect()
|
||||
{
|
||||
int ret = -EIO;
|
||||
int ret = -EIO;
|
||||
|
||||
/* read from the sensor */
|
||||
uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
uint8_t val[47] = { 0 };
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
ret = transfer(nullptr, 0, &val[0], 22);
|
||||
if (PX4FLOW_REG == 0x00) {
|
||||
ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
|
||||
}
|
||||
|
||||
if (ret < 0) {
|
||||
debug("error reading from sensor: %d", ret);
|
||||
|
@ -469,36 +475,74 @@ PX4FLOW::collect()
|
|||
return ret;
|
||||
}
|
||||
|
||||
// f.frame_count = val[1] << 8 | val[0];
|
||||
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
|
||||
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
|
||||
// f.flow_comp_m_x= val[7] << 8 | val[6];
|
||||
// f.flow_comp_m_y= val[9] << 8 | val[8];
|
||||
// f.qual= val[11] << 8 | val[10];
|
||||
// f.gyro_x_rate= val[13] << 8 | val[12];
|
||||
// f.gyro_y_rate= val[15] << 8 | val[14];
|
||||
// f.gyro_z_rate= val[17] << 8 | val[16];
|
||||
// f.gyro_range= val[18];
|
||||
// f.sonar_timestamp= val[19];
|
||||
// f.ground_distance= val[21] << 8 | val[20];
|
||||
if (PX4FLOW_REG == 0) {
|
||||
f.frame_count = val[1] << 8 | val[0];
|
||||
f.pixel_flow_x_sum = val[3] << 8 | val[2];
|
||||
f.pixel_flow_y_sum = val[5] << 8 | val[4];
|
||||
f.flow_comp_m_x = val[7] << 8 | val[6];
|
||||
f.flow_comp_m_y = val[9] << 8 | val[8];
|
||||
f.qual = val[11] << 8 | val[10];
|
||||
f.gyro_x_rate = val[13] << 8 | val[12];
|
||||
f.gyro_y_rate = val[15] << 8 | val[14];
|
||||
f.gyro_z_rate = val[17] << 8 | val[16];
|
||||
f.gyro_range = val[18];
|
||||
f.sonar_timestamp = val[19];
|
||||
f.ground_distance = val[21] << 8 | val[20];
|
||||
|
||||
f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
|
||||
f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
|
||||
f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
|
||||
f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
|
||||
f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
|
||||
f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
|
||||
f_integral.integration_timespan = val[37] << 24 | val[36] << 16
|
||||
| val[35] << 8 | val[34];
|
||||
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
|
||||
| val[39] << 8 | val[38];
|
||||
f_integral.ground_distance = val[43] << 8 | val[42];
|
||||
f_integral.gyro_temperature = val[45] << 8 | val[44];
|
||||
f_integral.qual = val[46];
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
|
||||
f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
|
||||
f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
|
||||
f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
|
||||
f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
|
||||
f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
|
||||
f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
|
||||
f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
|
||||
f_integral.ground_distance = val[21] << 8 | val[20];
|
||||
f_integral.gyro_temperature = val[23] << 8 | val[22];
|
||||
f_integral.qual = val[24];
|
||||
}
|
||||
|
||||
int16_t flowcx = val[7] << 8 | val[6];
|
||||
int16_t flowcy = val[9] << 8 | val[8];
|
||||
int16_t gdist = val[21] << 8 | val[20];
|
||||
|
||||
struct optical_flow_s report;
|
||||
report.flow_comp_x_m = float(flowcx) / 1000.0f;
|
||||
report.flow_comp_y_m = float(flowcy) / 1000.0f;
|
||||
report.flow_raw_x = val[3] << 8 | val[2];
|
||||
report.flow_raw_y = val[5] << 8 | val[4];
|
||||
report.ground_distance_m = float(gdist) / 1000.0f;
|
||||
report.quality = val[10];
|
||||
report.sensor_id = 0;
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
|
||||
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
|
||||
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
|
||||
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
|
||||
report.quality = f_integral.qual; //0:bad ; 255 max quality
|
||||
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
|
||||
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
|
||||
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
|
||||
report.integration_timespan = f_integral.integration_timespan; //microseconds
|
||||
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
|
||||
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
|
||||
|
||||
report.sensor_id = 0;
|
||||
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
|
||||
if (_px4flow_topic < 0) {
|
||||
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
|
||||
|
||||
} else {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
|
||||
}
|
||||
|
||||
/* post a report to the ring */
|
||||
if (_reports->force(&report)) {
|
||||
|
@ -558,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
|
|||
void
|
||||
PX4FLOW::cycle()
|
||||
{
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
debug("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&PX4FLOW::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure()) {
|
||||
debug("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
debug("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
|
||||
_measure_ticks);
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&PX4FLOW::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -647,14 +662,41 @@ start()
|
|||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new PX4FLOW(PX4FLOW_BUS);
|
||||
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
|
||||
#ifdef PX4_I2C_BUS_ESC
|
||||
delete g_dev;
|
||||
/* try 2nd bus */
|
||||
g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
#endif
|
||||
|
||||
delete g_dev;
|
||||
/* try 3rd bus */
|
||||
g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
#ifdef PX4_I2C_BUS_ESC
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
|
@ -683,7 +725,8 @@ fail:
|
|||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
void stop()
|
||||
void
|
||||
stop()
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
|
@ -714,6 +757,7 @@ test()
|
|||
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
|
||||
}
|
||||
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
|
@ -723,18 +767,18 @@ test()
|
|||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
|
||||
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
|
||||
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
|
||||
warnx("framecount_integral: %u",
|
||||
f_integral.frame_count_since_last_readout);
|
||||
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
/* start the sensor polling at 10Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
|
||||
errx(1, "failed to set 10Hz poll rate");
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
for (unsigned i = 0; i < 10; i++) {
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
|
@ -754,9 +798,22 @@ test()
|
|||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
|
||||
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
|
||||
warnx("framecount_total: %u", f.frame_count);
|
||||
warnx("framecount_integral: %u",
|
||||
f_integral.frame_count_since_last_readout);
|
||||
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
|
||||
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
|
||||
warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
|
||||
warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
|
||||
warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
|
||||
warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
|
||||
warnx("ground_distance: %0.2f m",
|
||||
(double) f_integral.ground_distance / 1000);
|
||||
warnx("time since last sonar update [us]: %i",
|
||||
f_integral.time_since_last_sonar_update);
|
||||
warnx("quality integration average : %i", f_integral.qual);
|
||||
warnx("quality : %i", f.qual);
|
||||
|
||||
|
||||
}
|
||||
|
|
|
@ -817,6 +817,11 @@ PX4IO::init()
|
|||
|
||||
}
|
||||
|
||||
/* set safety to off if circuit breaker enabled */
|
||||
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
|
||||
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
|
||||
}
|
||||
|
||||
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
||||
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
|
||||
|
||||
|
@ -1155,52 +1160,54 @@ PX4IO::io_set_arming_state()
|
|||
actuator_armed_s armed; ///< system armed state
|
||||
vehicle_control_mode_s control_mode; ///< vehicle_control_mode
|
||||
|
||||
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
|
||||
int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
|
||||
int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
|
||||
|
||||
uint16_t set = 0;
|
||||
uint16_t clear = 0;
|
||||
|
||||
if (armed.armed) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||
if (have_armed == OK) {
|
||||
if (armed.armed) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||
}
|
||||
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||
if (armed.lockdown && !_lockdown_override) {
|
||||
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||
}
|
||||
|
||||
/* Do not set failsafe if circuit breaker is enabled */
|
||||
if (armed.force_failsafe && !_cb_flighttermination) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
}
|
||||
|
||||
// XXX this is for future support in the commander
|
||||
// but can be removed if unneeded
|
||||
// if (armed.termination_failsafe) {
|
||||
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
|
||||
// } else {
|
||||
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
|
||||
// }
|
||||
|
||||
if (armed.ready_to_arm) {
|
||||
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||
}
|
||||
}
|
||||
|
||||
if (armed.lockdown && !_lockdown_override) {
|
||||
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||
}
|
||||
|
||||
/* Do not set failsafe if circuit breaker is enabled */
|
||||
if (armed.force_failsafe && !_cb_flighttermination) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
}
|
||||
|
||||
// XXX this is for future support in the commander
|
||||
// but can be removed if unneeded
|
||||
// if (armed.termination_failsafe) {
|
||||
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
|
||||
// } else {
|
||||
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
|
||||
// }
|
||||
|
||||
if (armed.ready_to_arm) {
|
||||
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||
}
|
||||
|
||||
if (control_mode.flag_external_manual_override_ok) {
|
||||
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
||||
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
||||
if (have_control_mode == OK) {
|
||||
if (control_mode.flag_external_manual_override_ok) {
|
||||
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
||||
}
|
||||
}
|
||||
|
||||
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
|
||||
|
@ -2193,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
|||
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
||||
if (pwm->channel_count > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
return -E2BIG;
|
||||
|
||||
/* copy values to registers in IO */
|
||||
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
|
||||
|
@ -2212,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
|||
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
||||
if (pwm->channel_count > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
return -E2BIG;
|
||||
|
||||
/* copy values to registers in IO */
|
||||
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
|
||||
|
@ -2231,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
|||
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
||||
if (pwm->channel_count > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
return -E2BIG;
|
||||
|
||||
/* copy values to registers in IO */
|
||||
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
|
||||
|
@ -2250,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
|||
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
||||
if (pwm->channel_count > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
return -E2BIG;
|
||||
|
||||
/* copy values to registers in IO */
|
||||
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
|
||||
|
@ -2587,9 +2594,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
|||
on param_get()
|
||||
*/
|
||||
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
|
||||
if (config->channel >= _max_actuators) {
|
||||
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
return -E2BIG;
|
||||
}
|
||||
|
||||
/* copy values to registers in IO */
|
||||
|
|
|
@ -121,7 +121,7 @@ private:
|
|||
/* for now, we only support one RGBLED */
|
||||
namespace
|
||||
{
|
||||
RGBLED *g_rgbled;
|
||||
RGBLED *g_rgbled = nullptr;
|
||||
}
|
||||
|
||||
void rgbled_usage();
|
||||
|
@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[])
|
|||
|
||||
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
|
||||
close(fd);
|
||||
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "rgb")) {
|
||||
if (argc < 5) {
|
||||
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
|
||||
|
|
|
@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_MAX - 10,
|
||||
2048,
|
||||
roboclaw_thread_main,
|
||||
(const char **)argv);
|
||||
(char * const *)argv);
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
|
|
|
@ -547,7 +547,7 @@ SF0X::collect()
|
|||
|
||||
float si_units;
|
||||
bool valid = false;
|
||||
|
||||
|
||||
for (int i = 0; i < ret; i++) {
|
||||
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
|
||||
valid = true;
|
||||
|
@ -566,6 +566,8 @@ SF0X::collect()
|
|||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.minimum_distance = get_minimum_distance();
|
||||
report.maximum_distance = get_maximum_distance();
|
||||
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
|
||||
|
||||
/* publish it */
|
||||
|
|
|
@ -253,9 +253,11 @@ static uint16_t latency_baseline;
|
|||
static uint16_t latency_actual;
|
||||
|
||||
/* latency histogram */
|
||||
#define LATENCY_BUCKET_COUNT 8
|
||||
static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
|
||||
static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
|
||||
#define LATENCY_BUCKET_COUNT 8
|
||||
__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
|
||||
__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
|
||||
__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
|
||||
|
||||
|
||||
/* timer-specific functions */
|
||||
static void hrt_tim_init(void);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 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
|
||||
|
@ -32,10 +32,13 @@
|
|||
############################################################################
|
||||
|
||||
#
|
||||
# Build flow speed control
|
||||
# Makefile to build the TeraRanger One range finder driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = flow_speed_control
|
||||
MODULE_COMMAND = trone
|
||||
|
||||
SRCS = flow_speed_control_main.c \
|
||||
flow_speed_control_params.c
|
||||
SRCS = trone.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
|
@ -0,0 +1,915 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file trone.cpp
|
||||
* @author Luis Rodrigues
|
||||
*
|
||||
* Driver for the TeraRanger One range finders connected via I2C.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
/* Configuration Constants */
|
||||
#define TRONE_BUS PX4_I2C_BUS_EXPANSION
|
||||
#define TRONE_BASEADDR 0x30 /* 7-bit address */
|
||||
#define TRONE_DEVICE_PATH "/dev/trone"
|
||||
|
||||
/* TRONE Registers addresses */
|
||||
|
||||
#define TRONE_MEASURE_REG 0x00 /* Measure range register */
|
||||
|
||||
/* Device limits */
|
||||
#define TRONE_MIN_DISTANCE (0.20f)
|
||||
#define TRONE_MAX_DISTANCE (14.00f)
|
||||
|
||||
#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
class TRONE : public device::I2C
|
||||
{
|
||||
public:
|
||||
TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR);
|
||||
virtual ~TRONE();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
float _min_distance;
|
||||
float _max_distance;
|
||||
work_s _work;
|
||||
RingBuffer *_reports;
|
||||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
int _class_instance;
|
||||
|
||||
orb_advert_t _range_finder_topic;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Set the min and max distance thresholds if you want the end points of the sensors
|
||||
* range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE
|
||||
* and TRONE_MAX_DISTANCE
|
||||
*/
|
||||
void set_minimum_distance(float min);
|
||||
void set_maximum_distance(float max);
|
||||
float get_minimum_distance();
|
||||
float get_maximum_distance();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void cycle();
|
||||
int measure();
|
||||
int collect();
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
|
||||
};
|
||||
|
||||
static const uint8_t crc_table[] = {
|
||||
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
|
||||
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
|
||||
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
|
||||
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
|
||||
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
|
||||
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
|
||||
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
|
||||
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
|
||||
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
|
||||
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
|
||||
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
|
||||
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
|
||||
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
|
||||
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
|
||||
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
|
||||
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
|
||||
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
|
||||
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
|
||||
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
|
||||
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
|
||||
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
|
||||
0xfa, 0xfd, 0xf4, 0xf3
|
||||
};
|
||||
|
||||
static uint8_t crc8(uint8_t *p, uint8_t len) {
|
||||
uint16_t i;
|
||||
uint16_t crc = 0x0;
|
||||
|
||||
while (len--) {
|
||||
i = (crc ^ *p++) & 0xFF;
|
||||
crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
|
||||
}
|
||||
|
||||
return crc & 0xFF;
|
||||
}
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int trone_main(int argc, char *argv[]);
|
||||
|
||||
TRONE::TRONE(int bus, int address) :
|
||||
I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000),
|
||||
_min_distance(TRONE_MIN_DISTANCE),
|
||||
_max_distance(TRONE_MAX_DISTANCE),
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
_collect_phase(false),
|
||||
_class_instance(-1),
|
||||
_range_finder_topic(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
|
||||
{
|
||||
// up the retries since the device misses the first measure attempts
|
||||
I2C::_retries = 3;
|
||||
|
||||
// enable debug() calls
|
||||
_debug_enabled = false;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
TRONE::~TRONE()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
|
||||
}
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
perf_free(_buffer_overflows);
|
||||
}
|
||||
|
||||
int
|
||||
TRONE::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
/* get a publish handle on the range finder topic */
|
||||
struct range_finder_report rf_report;
|
||||
measure();
|
||||
_reports->get(&rf_report);
|
||||
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
|
||||
|
||||
if (_range_finder_topic < 0) {
|
||||
debug("failed to create sensor_range_finder object. Did you start uOrb?");
|
||||
}
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
TRONE::probe()
|
||||
{
|
||||
return measure();
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::set_minimum_distance(float min)
|
||||
{
|
||||
_min_distance = min;
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::set_maximum_distance(float max)
|
||||
{
|
||||
_max_distance = max;
|
||||
}
|
||||
|
||||
float
|
||||
TRONE::get_minimum_distance()
|
||||
{
|
||||
return _min_distance;
|
||||
}
|
||||
|
||||
float
|
||||
TRONE::get_maximum_distance()
|
||||
{
|
||||
return _max_distance;
|
||||
}
|
||||
|
||||
int
|
||||
TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0) {
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
}
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
irqrestore(flags);
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _reports->size();
|
||||
|
||||
case SENSORIOCRESET:
|
||||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
|
||||
case RANGEFINDERIOCSETMINIUMDISTANCE: {
|
||||
set_minimum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
|
||||
set_maximum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return I2C::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
TRONE::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct range_finder_report);
|
||||
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
|
||||
/*
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
* Note that we may be pre-empted by the workq thread while we are doing this;
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(rbuf)) {
|
||||
ret += sizeof(*rbuf);
|
||||
rbuf++;
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
do {
|
||||
_reports->flush();
|
||||
|
||||
/* trigger a measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* wait for it to complete */
|
||||
usleep(TRONE_CONVERSION_INTERVAL);
|
||||
|
||||
/* run the collection phase */
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(rbuf)) {
|
||||
ret = sizeof(*rbuf);
|
||||
}
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
TRONE::measure()
|
||||
{
|
||||
int ret;
|
||||
|
||||
/*
|
||||
* Send the command to begin a measurement.
|
||||
*/
|
||||
const uint8_t cmd = TRONE_MEASURE_REG;
|
||||
ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
TRONE::collect()
|
||||
{
|
||||
int ret = -EIO;
|
||||
|
||||
/* read from the sensor */
|
||||
uint8_t val[3] = {0, 0, 0};
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
ret = transfer(nullptr, 0, &val[0], 3);
|
||||
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint16_t distance = (val[0] << 8) | val[1];
|
||||
float si_units = distance * 0.001f; /* mm to m */
|
||||
struct range_finder_report report;
|
||||
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.minimum_distance = get_minimum_distance();
|
||||
report.maximum_distance = get_maximum_distance();
|
||||
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
|
||||
|
||||
|
||||
/* publish it, if we are the primary */
|
||||
if (_range_finder_topic >= 0) {
|
||||
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
|
||||
}
|
||||
|
||||
if (_reports->force(&report)) {
|
||||
perf_count(_buffer_overflows);
|
||||
}
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
ret = OK;
|
||||
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_reports->flush();
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::stop()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::cycle_trampoline(void *arg)
|
||||
{
|
||||
TRONE *dev = (TRONE *)arg;
|
||||
|
||||
dev->cycle();
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::cycle()
|
||||
{
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&TRONE::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure()) {
|
||||
log("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&TRONE::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(TRONE_CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
void
|
||||
TRONE::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
_reports->print_info("report queue");
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace trone
|
||||
{
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
const int ERROR = -1;
|
||||
|
||||
TRONE *g_dev;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start()
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new TRONE(TRONE_BUS);
|
||||
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(TRONE_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
void stop()
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
|
||||
} else {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform some basic functional tests on the driver;
|
||||
* make sure we can collect data from the sensor in polled
|
||||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test()
|
||||
{
|
||||
struct range_finder_report report;
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
|
||||
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH);
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "immediate read failed");
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("measurement: %0.2f m", (double)report.distance);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
}
|
||||
|
||||
/* read the sensor 50x and report each value */
|
||||
for (unsigned i = 0; i < 50; i++) {
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1) {
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "periodic read failed");
|
||||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("valid %u", report.valid);
|
||||
warnx("measurement: %0.3f", (double)report.distance);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
}
|
||||
|
||||
/* reset the sensor polling to default rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
|
||||
errx(1, "failed to set default poll rate");
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
err(1, "driver reset failed");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
err(1, "driver poll restart failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
trone_main(int argc, char *argv[])
|
||||
{
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
trone::start();
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
trone::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
trone::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
trone::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
|
||||
trone::info();
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
}
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* 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
|
||||
|
@ -31,6 +30,7 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file main.c
|
||||
*
|
||||
|
@ -55,7 +55,7 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
@ -106,11 +106,9 @@ static void usage(const char *reason);
|
|||
*
|
||||
* @param att_sp The current attitude setpoint - the values the system would like to reach.
|
||||
* @param att The current attitude. The controller should make the attitude match the setpoint
|
||||
* @param speed_body The velocity of the system. Currently unused.
|
||||
* @param rates_sp The angular rate setpoint. This is the output of the controller.
|
||||
*/
|
||||
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
|
||||
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
|
||||
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
|
||||
struct actuator_controls_s *actuators);
|
||||
|
||||
/**
|
||||
|
@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
|
|||
* @param att The current attitude
|
||||
* @param att_sp The attitude setpoint. This is the output of the controller
|
||||
*/
|
||||
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
|
||||
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
|
||||
|
||||
/* Variables */
|
||||
|
@ -135,8 +133,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
|
|||
static struct params p;
|
||||
static struct param_handles ph;
|
||||
|
||||
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
|
||||
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
|
||||
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
|
||||
struct actuator_controls_s *actuators)
|
||||
{
|
||||
|
||||
|
@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
|
|||
actuators->control[1] = pitch_err * p.pitch_p;
|
||||
}
|
||||
|
||||
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
|
||||
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
|
||||
{
|
||||
|
||||
|
@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
|
|||
/* calculate heading error */
|
||||
float yaw_err = att->yaw - bearing;
|
||||
/* apply control gain */
|
||||
float roll_command = yaw_err * p.hdng_p;
|
||||
att_sp->roll_body = yaw_err * p.hdng_p;
|
||||
|
||||
/* limit output, this commonly is a tuning parameter, too */
|
||||
if (att_sp->roll_body < -0.6f) {
|
||||
|
@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
|
|||
memset(&manual_sp, 0, sizeof(manual_sp));
|
||||
struct vehicle_status_s vstatus;
|
||||
memset(&vstatus, 0, sizeof(vstatus));
|
||||
struct vehicle_global_position_setpoint_s global_sp;
|
||||
struct position_setpoint_s global_sp;
|
||||
memset(&global_sp, 0, sizeof(global_sp));
|
||||
|
||||
/* output structs - this is what is sent to the mixer */
|
||||
|
@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
|
|||
|
||||
/* subscribe to topics. */
|
||||
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 vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* Setup of loop */
|
||||
float speed_body[3] = {0.0f, 0.0f, 0.0f};
|
||||
/* RC failsafe check */
|
||||
bool throttle_half_once = false;
|
||||
|
||||
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
|
||||
{ .fd = att_sub, .events = POLLIN }};
|
||||
|
||||
|
@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
|
|||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
if (global_sp_updated)
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
|
||||
|
||||
/* currently speed in body frame is not used, but here for reference */
|
||||
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;
|
||||
|
||||
warnx("Did not get a valid R\n");
|
||||
}
|
||||
if (global_sp_updated) {
|
||||
struct position_setpoint_triplet_s triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet);
|
||||
memcpy(&global_sp, &triplet.current, sizeof(global_sp));
|
||||
}
|
||||
|
||||
if (manual_sp_updated)
|
||||
|
@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
||||
|
||||
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
|
||||
if (isfinite(manual_sp.throttle) &&
|
||||
(manual_sp.throttle >= 0.6f) &&
|
||||
(manual_sp.throttle <= 1.0f)) {
|
||||
throttle_half_once = true;
|
||||
if (isfinite(manual_sp.z) &&
|
||||
(manual_sp.z >= 0.6f) &&
|
||||
(manual_sp.z <= 1.0f)) {
|
||||
}
|
||||
|
||||
/* get the system status and the flight mode we're in */
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
|
||||
/* control */
|
||||
|
||||
#warning fix this
|
||||
#if 0
|
||||
if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
|
||||
vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
|
||||
|
||||
/* simple heading control */
|
||||
control_heading(&global_pos, &global_sp, &att, &att_sp);
|
||||
|
||||
/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
|
||||
actuators.control[1] = 0.0f;
|
||||
actuators.control[2] = 0.0f;
|
||||
|
||||
/* simple attitude control */
|
||||
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
|
||||
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* set flaps to zero */
|
||||
actuators.control[4] = 0.0f;
|
||||
|
||||
} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
|
||||
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
|
||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (vstatus.rc_signal_lost && throttle_half_once) {
|
||||
|
||||
/* 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();
|
||||
|
||||
/* attitude control */
|
||||
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
|
||||
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* pass through flaps */
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* publish rates */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
|
||||
|
||||
|
@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
|
|||
isfinite(actuators.control[2]) &&
|
||||
isfinite(actuators.control[3])) {
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
if (verbose) {
|
||||
warnx("published");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -532,7 +423,7 @@ int ex_fixedwing_control_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
fixedwing_control_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
|
|
@ -41,3 +41,5 @@ SRCS = main.c \
|
|||
params.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
EXTRACFLAGS = -Wframe-larger-than=1200
|
||||
|
|
|
@ -114,7 +114,7 @@ int flow_position_estimator_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_MAX - 5,
|
||||
4000,
|
||||
flow_position_estimator_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
|||
if (vehicle_liftoff || params.debug)
|
||||
{
|
||||
/* copy flow */
|
||||
flow_speed[0] = flow.flow_comp_x_m;
|
||||
flow_speed[1] = flow.flow_comp_y_m;
|
||||
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
|
||||
flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
|
||||
flow_speed[2] = 0.0f;
|
||||
|
||||
/* convert to bodyframe velocity */
|
||||
|
|
|
@ -39,3 +39,5 @@ MODULE_COMMAND = flow_position_estimator
|
|||
|
||||
SRCS = flow_position_estimator_main.c \
|
||||
flow_position_estimator_params.c
|
||||
|
||||
EXTRACFLAGS = -Wno-float-equal
|
||||
|
|
|
@ -1,387 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file flow_speed_control.c
|
||||
*
|
||||
* Optical flow speed controller
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
#include <termios.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
|
||||
#include <uORB/topics/filtered_bottom_flow.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <poll.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "flow_speed_control_params.h"
|
||||
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
__EXPORT int flow_speed_control_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of position controller.
|
||||
*/
|
||||
static int flow_speed_control_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_spawn_cmd().
|
||||
*/
|
||||
int flow_speed_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start"))
|
||||
{
|
||||
if (thread_running)
|
||||
{
|
||||
printf("flow speed control already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd("flow_speed_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 6,
|
||||
4096,
|
||||
flow_speed_control_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
{
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status"))
|
||||
{
|
||||
if (thread_running)
|
||||
printf("\tflow speed control app is running\n");
|
||||
else
|
||||
printf("\tflow speed control app not started\n");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static int
|
||||
flow_speed_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* welcome user */
|
||||
thread_running = true;
|
||||
static int mavlink_fd;
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
mavlink_log_info(mavlink_fd,"[fsc] started");
|
||||
|
||||
uint32_t counter = 0;
|
||||
|
||||
/* structures */
|
||||
struct actuator_armed_s armed;
|
||||
memset(&armed, 0, sizeof(armed));
|
||||
struct vehicle_control_mode_s control_mode;
|
||||
memset(&control_mode, 0, sizeof(control_mode));
|
||||
struct filtered_bottom_flow_s filtered_flow;
|
||||
memset(&filtered_flow, 0, sizeof(filtered_flow));
|
||||
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
|
||||
memset(&speed_sp, 0, sizeof(speed_sp));
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
|
||||
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
|
||||
|
||||
orb_advert_t att_sp_pub;
|
||||
bool attitude_setpoint_adverted = false;
|
||||
|
||||
/* parameters init*/
|
||||
struct flow_speed_control_params params;
|
||||
struct flow_speed_control_param_handles param_handles;
|
||||
parameters_init(¶m_handles);
|
||||
parameters_update(¶m_handles, ¶ms);
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
|
||||
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
|
||||
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
|
||||
|
||||
static bool sensors_ready = false;
|
||||
static bool status_changed = false;
|
||||
|
||||
while (!thread_should_exit)
|
||||
{
|
||||
/* wait for first attitude msg to be sure all data are available */
|
||||
if (sensors_ready)
|
||||
{
|
||||
/* polling */
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
|
||||
{ .fd = parameter_update_sub, .events = POLLIN }
|
||||
};
|
||||
|
||||
/* wait for a position update, check for exit condition every 5000 ms */
|
||||
int ret = poll(fds, 2, 500);
|
||||
|
||||
if (ret < 0)
|
||||
{
|
||||
/* poll error, count it in perf */
|
||||
perf_count(mc_err_perf);
|
||||
}
|
||||
else if (ret == 0)
|
||||
{
|
||||
/* no return value, ignore */
|
||||
// printf("[flow speed control] no bodyframe speed setpoints updates\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
/* parameter update available? */
|
||||
if (fds[1].revents & POLLIN)
|
||||
{
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
|
||||
|
||||
parameters_update(¶m_handles, ¶ms);
|
||||
mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
|
||||
}
|
||||
|
||||
/* only run controller if position/speed changed */
|
||||
if (fds[0].revents & POLLIN)
|
||||
{
|
||||
perf_begin(mc_loop_perf);
|
||||
|
||||
/* get a local copy of the armed topic */
|
||||
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||
/* get a local copy of the control mode */
|
||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||
/* get a local copy of filtered bottom flow */
|
||||
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
|
||||
/* get a local copy of bodyframe speed setpoint */
|
||||
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
|
||||
/* get a local copy of control mode */
|
||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||
|
||||
if (control_mode.flag_control_velocity_enabled)
|
||||
{
|
||||
/* calc new roll/pitch */
|
||||
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
|
||||
float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p;
|
||||
|
||||
if(status_changed == false)
|
||||
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
|
||||
|
||||
status_changed = true;
|
||||
|
||||
/* limit roll and pitch corrections */
|
||||
if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
|
||||
{
|
||||
att_sp.pitch_body = pitch_body;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(pitch_body > params.limit_pitch)
|
||||
att_sp.pitch_body = params.limit_pitch;
|
||||
if(pitch_body < -params.limit_pitch)
|
||||
att_sp.pitch_body = -params.limit_pitch;
|
||||
}
|
||||
|
||||
if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
|
||||
{
|
||||
att_sp.roll_body = roll_body;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(roll_body > params.limit_roll)
|
||||
att_sp.roll_body = params.limit_roll;
|
||||
if(roll_body < -params.limit_roll)
|
||||
att_sp.roll_body = -params.limit_roll;
|
||||
}
|
||||
|
||||
/* set yaw setpoint forward*/
|
||||
att_sp.yaw_body = speed_sp.yaw_sp;
|
||||
|
||||
/* add trim from parameters */
|
||||
att_sp.roll_body = att_sp.roll_body + params.trim_roll;
|
||||
att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
|
||||
|
||||
att_sp.thrust = speed_sp.thrust_sp;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* publish new attitude setpoint */
|
||||
if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
|
||||
{
|
||||
if (attitude_setpoint_adverted)
|
||||
{
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
else
|
||||
{
|
||||
att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
attitude_setpoint_adverted = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
warnx("NaN in flow speed controller!");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(status_changed == true)
|
||||
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
|
||||
|
||||
status_changed = false;
|
||||
|
||||
/* reset attitude setpoint */
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
att_sp.thrust = 0.0f;
|
||||
att_sp.yaw_body = 0.0f;
|
||||
}
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(mc_interval_perf);
|
||||
perf_end(mc_loop_perf);
|
||||
}
|
||||
}
|
||||
|
||||
counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* sensors not ready waiting for first attitude msg */
|
||||
|
||||
/* polling */
|
||||
struct pollfd fds[1] = {
|
||||
{ .fd = vehicle_attitude_sub, .events = POLLIN },
|
||||
};
|
||||
|
||||
/* wait for a flow msg, check for exit condition every 5 s */
|
||||
int ret = poll(fds, 1, 5000);
|
||||
|
||||
if (ret < 0)
|
||||
{
|
||||
/* poll error, count it in perf */
|
||||
perf_count(mc_err_perf);
|
||||
}
|
||||
else if (ret == 0)
|
||||
{
|
||||
/* no return value, ignore */
|
||||
mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
|
||||
}
|
||||
else
|
||||
{
|
||||
if (fds[0].revents & POLLIN)
|
||||
{
|
||||
sensors_ready = true;
|
||||
mavlink_log_info(mavlink_fd,"[fsp] initialized.");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd,"[fsc] ending now...");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
close(parameter_update_sub);
|
||||
close(vehicle_attitude_sub);
|
||||
close(vehicle_bodyframe_speed_setpoint_sub);
|
||||
close(filtered_bottom_flow_sub);
|
||||
close(armed_sub);
|
||||
close(control_mode_sub);
|
||||
close(att_sp_pub);
|
||||
|
||||
perf_print_counter(mc_loop_perf);
|
||||
perf_free(mc_loop_perf);
|
||||
|
||||
fflush(stdout);
|
||||
return 0;
|
||||
}
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* 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
|
||||
|
@ -34,41 +33,94 @@
|
|||
/**
|
||||
* @file hwtest.c
|
||||
*
|
||||
* Simple functional hardware test.
|
||||
* Simple output test.
|
||||
* @ref Documentation https://pixhawk.org/dev/examples/write_output
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <nuttx/config.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
__EXPORT int ex_hwtest_main(int argc, char *argv[]);
|
||||
|
||||
int ex_hwtest_main(int argc, char *argv[])
|
||||
{
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
|
||||
warnx("DO NOT FORGET TO STOP THE COMMANDER APP!");
|
||||
warnx("(run <commander stop> to do so)");
|
||||
warnx("usage: http://px4.io/dev/examples/write_output");
|
||||
|
||||
int i;
|
||||
float rcvalue = -1.0f;
|
||||
hrt_abstime stime;
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
|
||||
|
||||
while (true) {
|
||||
stime = hrt_absolute_time();
|
||||
while (hrt_absolute_time() - stime < 1000000) {
|
||||
for (i=0; i<8; i++)
|
||||
actuators.control[i] = rcvalue;
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
|
||||
}
|
||||
warnx("servos set to %.1f", rcvalue);
|
||||
rcvalue *= -1.0f;
|
||||
}
|
||||
struct actuator_armed_s arm;
|
||||
memset(&arm, 0 , sizeof(arm));
|
||||
|
||||
return OK;
|
||||
arm.timestamp = hrt_absolute_time();
|
||||
arm.ready_to_arm = true;
|
||||
arm.armed = true;
|
||||
orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm);
|
||||
orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm);
|
||||
|
||||
/* read back values to validate */
|
||||
int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
|
||||
orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);
|
||||
|
||||
if (arm.ready_to_arm && arm.armed) {
|
||||
warnx("Actuator armed");
|
||||
|
||||
} else {
|
||||
errx(1, "Arming actuators failed");
|
||||
}
|
||||
|
||||
hrt_abstime stime;
|
||||
|
||||
int count = 0;
|
||||
|
||||
while (count != 36) {
|
||||
stime = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - stime < 1000000) {
|
||||
for (int i = 0; i != 2; i++) {
|
||||
if (count <= 5) {
|
||||
actuators.control[i] = -1.0f;
|
||||
|
||||
} else if (count <= 10) {
|
||||
actuators.control[i] = -0.7f;
|
||||
|
||||
} else if (count <= 15) {
|
||||
actuators.control[i] = -0.5f;
|
||||
|
||||
} else if (count <= 20) {
|
||||
actuators.control[i] = -0.3f;
|
||||
|
||||
} else if (count <= 25) {
|
||||
actuators.control[i] = 0.0f;
|
||||
|
||||
} else if (count <= 30) {
|
||||
actuators.control[i] = 0.3f;
|
||||
|
||||
} else {
|
||||
actuators.control[i] = 0.5f;
|
||||
}
|
||||
}
|
||||
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
|
||||
usleep(10000);
|
||||
}
|
||||
|
||||
warnx("count %i", count);
|
||||
count++;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
|
@ -107,7 +107,7 @@ int matlab_csv_serial_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
matlab_csv_serial_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -38,10 +38,13 @@
|
|||
* @author Example User <mail@example.com>
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
|
@ -100,7 +103,7 @@ int px4_daemon_app_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_DEFAULT,
|
||||
2000,
|
||||
px4_daemon_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -181,6 +181,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
|
|||
x = tmp;
|
||||
return;
|
||||
}
|
||||
case ROTATION_ROLL_270_YAW_270: {
|
||||
tmp = z; z = -y; y = tmp;
|
||||
tmp = x; x = y; y = -tmp;
|
||||
return;
|
||||
}
|
||||
case ROTATION_PITCH_90: {
|
||||
tmp = z; z = -x; x = tmp;
|
||||
return;
|
||||
|
|
|
@ -72,7 +72,7 @@ const char *decode_states[] = {"UNSYNCED",
|
|||
#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
|
||||
|
||||
static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
|
||||
static unsigned _rxlen;
|
||||
static uint8_t _rxlen;
|
||||
|
||||
static ReceiverFcPacket _rxpacket;
|
||||
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 1efd24427539fa332a15151143466ec760fa5fff
|
|
@ -0,0 +1,298 @@
|
|||
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
|
||||
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
|
||||
|
||||
|
||||
%LQG Postion Estimator and Controller
|
||||
% Observer:
|
||||
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
|
||||
% x[n+1|n] = Ax[n|n] + Bu[n]
|
||||
%
|
||||
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
|
||||
%
|
||||
%
|
||||
% Arguments:
|
||||
% approx_prediction: if 1 then the exponential map is approximated with a
|
||||
% first order taylor approximation. has at the moment not a big influence
|
||||
% (just 1st or 2nd order approximation) we should change it to rodriquez
|
||||
% approximation.
|
||||
% use_inertia_matrix: set to true if you have the inertia matrix J for your
|
||||
% quadrotor
|
||||
% xa_apo_k: old state vectotr
|
||||
% zFlag: if sensor measurement is available [gyro, acc, mag]
|
||||
% dt: dt in s
|
||||
% z: measurements [gyro, acc, mag]
|
||||
% q_rotSpeed: process noise gyro
|
||||
% q_rotAcc: process noise gyro acceleration
|
||||
% q_acc: process noise acceleration
|
||||
% q_mag: process noise magnetometer
|
||||
% r_gyro: measurement noise gyro
|
||||
% r_accel: measurement noise accel
|
||||
% r_mag: measurement noise mag
|
||||
% J: moment of inertia matrix
|
||||
|
||||
|
||||
% Output:
|
||||
% xa_apo: updated state vectotr
|
||||
% Pa_apo: updated state covariance matrix
|
||||
% Rot_matrix: rotation matrix
|
||||
% eulerAngles: euler angles
|
||||
% debugOutput: not used
|
||||
|
||||
|
||||
%% model specific parameters
|
||||
|
||||
|
||||
|
||||
% compute once the inverse of the Inertia
|
||||
persistent Ji;
|
||||
if isempty(Ji)
|
||||
Ji=single(inv(J));
|
||||
end
|
||||
|
||||
%% init
|
||||
persistent x_apo
|
||||
if(isempty(x_apo))
|
||||
gyro_init=single([0;0;0]);
|
||||
gyro_acc_init=single([0;0;0]);
|
||||
acc_init=single([0;0;-9.81]);
|
||||
mag_init=single([1;0;0]);
|
||||
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
|
||||
|
||||
end
|
||||
|
||||
persistent P_apo
|
||||
if(isempty(P_apo))
|
||||
% P_apo = single(eye(NSTATES) * 1000);
|
||||
P_apo = single(200*ones(12));
|
||||
end
|
||||
|
||||
debugOutput = single(zeros(4,1));
|
||||
|
||||
%% copy the states
|
||||
wx= x_apo(1); % x body angular rate
|
||||
wy= x_apo(2); % y body angular rate
|
||||
wz= x_apo(3); % z body angular rate
|
||||
|
||||
wax= x_apo(4); % x body angular acceleration
|
||||
way= x_apo(5); % y body angular acceleration
|
||||
waz= x_apo(6); % z body angular acceleration
|
||||
|
||||
zex= x_apo(7); % x component gravity vector
|
||||
zey= x_apo(8); % y component gravity vector
|
||||
zez= x_apo(9); % z component gravity vector
|
||||
|
||||
mux= x_apo(10); % x component magnetic field vector
|
||||
muy= x_apo(11); % y component magnetic field vector
|
||||
muz= x_apo(12); % z component magnetic field vector
|
||||
|
||||
|
||||
|
||||
|
||||
%% prediction section
|
||||
% compute the apriori state estimate from the previous aposteriori estimate
|
||||
%body angular accelerations
|
||||
if (use_inertia_matrix==1)
|
||||
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
|
||||
else
|
||||
wak =[wax;way;waz];
|
||||
end
|
||||
|
||||
%body angular rates
|
||||
wk =[wx; wy; wz] + dt*wak;
|
||||
|
||||
%derivative of the prediction rotation matrix
|
||||
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
|
||||
|
||||
%prediction of the earth z vector
|
||||
if (approx_prediction==1)
|
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2
|
||||
% so we do a first order approximation of the exponential map
|
||||
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
|
||||
|
||||
else
|
||||
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
|
||||
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
|
||||
%precision
|
||||
end
|
||||
|
||||
|
||||
|
||||
%prediction of the magnetic vector
|
||||
if (approx_prediction==1)
|
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2
|
||||
% so we do a first order approximation of the exponential map
|
||||
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
|
||||
else
|
||||
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
|
||||
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
|
||||
%precision
|
||||
end
|
||||
|
||||
x_apr=[wk;wak;zek;muk];
|
||||
|
||||
% compute the apriori error covariance estimate from the previous
|
||||
%aposteriori estimate
|
||||
|
||||
EZ=[0,zez,-zey;
|
||||
-zez,0,zex;
|
||||
zey,-zex,0]';
|
||||
MA=[0,muz,-muy;
|
||||
-muz,0,mux;
|
||||
muy,-mux,0]';
|
||||
|
||||
E=single(eye(3));
|
||||
Z=single(zeros(3));
|
||||
|
||||
A_lin=[ Z, E, Z, Z
|
||||
Z, Z, Z, Z
|
||||
EZ, Z, O, Z
|
||||
MA, Z, Z, O];
|
||||
|
||||
A_lin=eye(12)+A_lin*dt;
|
||||
|
||||
%process covariance matrix
|
||||
|
||||
persistent Q
|
||||
if (isempty(Q))
|
||||
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
|
||||
q_rotAcc,q_rotAcc,q_rotAcc,...
|
||||
q_acc,q_acc,q_acc,...
|
||||
q_mag,q_mag,q_mag]);
|
||||
end
|
||||
|
||||
P_apr=A_lin*P_apo*A_lin'+Q;
|
||||
|
||||
|
||||
%% update
|
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
|
||||
|
||||
% R=[r_gyro,0,0,0,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0,0,0,0;
|
||||
% 0,0,0,r_accel,0,0,0,0,0;
|
||||
% 0,0,0,0,r_accel,0,0,0,0;
|
||||
% 0,0,0,0,0,r_accel,0,0,0;
|
||||
% 0,0,0,0,0,0,r_mag,0,0;
|
||||
% 0,0,0,0,0,0,0,r_mag,0;
|
||||
% 0,0,0,0,0,0,0,0,r_mag];
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
|
||||
%observation matrix
|
||||
%[zw;ze;zmk];
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, E, Z;
|
||||
Z, Z, Z, E];
|
||||
|
||||
y_k=z(1:9)-H_k*x_apr;
|
||||
|
||||
|
||||
%S_k=H_k*P_apr*H_k'+R;
|
||||
S_k=H_k*P_apr*H_k';
|
||||
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
|
||||
K_k=(P_apr*H_k'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k)*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
|
||||
|
||||
R=[r_gyro,0,0;
|
||||
0,r_gyro,0;
|
||||
0,0,r_gyro];
|
||||
R_v=[r_gyro,r_gyro,r_gyro];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z];
|
||||
|
||||
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
|
||||
|
||||
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
|
||||
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
|
||||
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
|
||||
|
||||
% R=[r_gyro,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0;
|
||||
% 0,0,0,r_accel,0,0;
|
||||
% 0,0,0,0,r_accel,0;
|
||||
% 0,0,0,0,0,r_accel];
|
||||
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, E, Z];
|
||||
|
||||
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
|
||||
|
||||
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
|
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
|
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
|
||||
% R=[r_gyro,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0;
|
||||
% 0,0,0,r_mag,0,0;
|
||||
% 0,0,0,0,r_mag,0;
|
||||
% 0,0,0,0,0,r_mag];
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, Z, E];
|
||||
|
||||
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
|
||||
|
||||
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
|
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
|
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
|
||||
else
|
||||
x_apo=x_apr;
|
||||
P_apo=P_apr;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
%% euler anglels extraction
|
||||
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
|
||||
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
|
||||
|
||||
y_n_b=cross(z_n_b,m_n_b);
|
||||
y_n_b=y_n_b./norm(y_n_b);
|
||||
|
||||
x_n_b=(cross(y_n_b,z_n_b));
|
||||
x_n_b=x_n_b./norm(x_n_b);
|
||||
|
||||
|
||||
xa_apo=x_apo;
|
||||
Pa_apo=P_apo;
|
||||
% rotation matrix from earth to body system
|
||||
Rot_matrix=[x_n_b,y_n_b,z_n_b];
|
||||
|
||||
|
||||
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
|
||||
theta=-asin(Rot_matrix(1,3));
|
||||
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
|
||||
eulerAngles=[phi;theta;psi];
|
||||
|
|
@ -0,0 +1,502 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
|
||||
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
|
||||
<profile key="profile.mex">
|
||||
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
|
||||
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
|
||||
<param.RanInstrumentedMex>false</param.RanInstrumentedMex>
|
||||
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
|
||||
<param.SpecifiedWorkingFolder />
|
||||
<param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
|
||||
<param.SpecifiedBuildFolder />
|
||||
<param.SearchPaths />
|
||||
<param.ResponsivenessChecks>true</param.ResponsivenessChecks>
|
||||
<param.ExtrinsicCalls>true</param.ExtrinsicCalls>
|
||||
<param.IntegrityChecks>true</param.IntegrityChecks>
|
||||
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
|
||||
<param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
|
||||
<param.EnableVariableSizing>true</param.EnableVariableSizing>
|
||||
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
|
||||
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
|
||||
<param.StackUsageMax>200000</param.StackUsageMax>
|
||||
<param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
|
||||
<param.GenerateComments>true</param.GenerateComments>
|
||||
<param.MATLABSourceComments>false</param.MATLABSourceComments>
|
||||
<param.ReservedNameArray />
|
||||
<param.EnableScreener>true</param.EnableScreener>
|
||||
<param.EnableDebugging>false</param.EnableDebugging>
|
||||
<param.GenerateReport>true</param.GenerateReport>
|
||||
<param.LaunchReport>false</param.LaunchReport>
|
||||
<param.CustomSourceCode />
|
||||
<param.CustomHeaderCode />
|
||||
<param.CustomInitializer />
|
||||
<param.CustomTerminator />
|
||||
<param.CustomInclude />
|
||||
<param.CustomSource />
|
||||
<param.CustomLibrary />
|
||||
<param.PostCodeGenCommand />
|
||||
<param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
|
||||
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
|
||||
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
|
||||
<param.RecursionLimit>100</param.RecursionLimit>
|
||||
<param.TargetLang>option.TargetLang.C</param.TargetLang>
|
||||
<param.EchoExpressions>true</param.EchoExpressions>
|
||||
<param.InlineThreshold>10</param.InlineThreshold>
|
||||
<param.InlineThresholdMax>200</param.InlineThresholdMax>
|
||||
<param.InlineStackLimit>4000</param.InlineStackLimit>
|
||||
<param.EnableMemcpy>true</param.EnableMemcpy>
|
||||
<param.MemcpyThreshold>64</param.MemcpyThreshold>
|
||||
<param.EnableOpenMP>true</param.EnableOpenMP>
|
||||
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
|
||||
<param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
|
||||
<unset>
|
||||
<param.MergeInstrumentationResults />
|
||||
<param.BuiltInstrumentedMex />
|
||||
<param.RanInstrumentedMex />
|
||||
<param.WorkingFolder />
|
||||
<param.SpecifiedWorkingFolder />
|
||||
<param.BuildFolder />
|
||||
<param.SpecifiedBuildFolder />
|
||||
<param.SearchPaths />
|
||||
<param.ResponsivenessChecks />
|
||||
<param.ExtrinsicCalls />
|
||||
<param.IntegrityChecks />
|
||||
<param.SaturateOnIntegerOverflow />
|
||||
<param.GlobalDataSyncMethod />
|
||||
<param.EnableVariableSizing />
|
||||
<param.DynamicMemoryAllocation />
|
||||
<param.DynamicMemoryAllocationThreshold />
|
||||
<param.StackUsageMax />
|
||||
<param.FilePartitionMethod />
|
||||
<param.GenerateComments />
|
||||
<param.MATLABSourceComments />
|
||||
<param.ReservedNameArray />
|
||||
<param.EnableScreener />
|
||||
<param.EnableDebugging />
|
||||
<param.GenerateReport />
|
||||
<param.LaunchReport />
|
||||
<param.CustomSourceCode />
|
||||
<param.CustomHeaderCode />
|
||||
<param.CustomInitializer />
|
||||
<param.CustomTerminator />
|
||||
<param.CustomInclude />
|
||||
<param.CustomSource />
|
||||
<param.CustomLibrary />
|
||||
<param.PostCodeGenCommand />
|
||||
<param.ProposeFixedPointDataTypes />
|
||||
<param.mex.GenCodeOnly />
|
||||
<param.ConstantFoldingTimeout />
|
||||
<param.RecursionLimit />
|
||||
<param.TargetLang />
|
||||
<param.EchoExpressions />
|
||||
<param.InlineThreshold />
|
||||
<param.InlineThresholdMax />
|
||||
<param.InlineStackLimit />
|
||||
<param.EnableMemcpy />
|
||||
<param.MemcpyThreshold />
|
||||
<param.EnableOpenMP />
|
||||
<param.InitFltsAndDblsToZero />
|
||||
<param.ConstantInputs />
|
||||
</unset>
|
||||
</profile>
|
||||
<profile key="profile.c">
|
||||
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
|
||||
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
|
||||
<param.SpecifiedWorkingFolder />
|
||||
<param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
|
||||
<param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
|
||||
<param.SearchPaths />
|
||||
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
|
||||
<param.PurelyIntegerCode>false</param.PurelyIntegerCode>
|
||||
<param.SupportNonFinite>false</param.SupportNonFinite>
|
||||
<param.EnableVariableSizing>false</param.EnableVariableSizing>
|
||||
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
|
||||
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
|
||||
<param.StackUsageMax>4000</param.StackUsageMax>
|
||||
<param.MultiInstanceCode>false</param.MultiInstanceCode>
|
||||
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
|
||||
<param.GenerateComments>true</param.GenerateComments>
|
||||
<param.MATLABSourceComments>true</param.MATLABSourceComments>
|
||||
<param.MATLABFcnDesc>false</param.MATLABFcnDesc>
|
||||
<param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
|
||||
<param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
|
||||
<param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
|
||||
<param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
|
||||
<param.MaxIdLength>31</param.MaxIdLength>
|
||||
<param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
|
||||
<param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
|
||||
<param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
|
||||
<param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
|
||||
<param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
|
||||
<param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
|
||||
<param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
|
||||
<param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
|
||||
<param.ReservedNameArray />
|
||||
<param.EnableScreener>true</param.EnableScreener>
|
||||
<param.Verbose>false</param.Verbose>
|
||||
<param.GenerateReport>true</param.GenerateReport>
|
||||
<param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
|
||||
<param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
|
||||
<param.LaunchReport>true</param.LaunchReport>
|
||||
<param.CustomSourceCode />
|
||||
<param.CustomHeaderCode />
|
||||
<param.CustomInitializer />
|
||||
<param.CustomTerminator />
|
||||
<param.CustomInclude />
|
||||
<param.CustomSource />
|
||||
<param.CustomLibrary />
|
||||
<param.PostCodeGenCommand />
|
||||
<param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
|
||||
<param.SameHardware>true</param.SameHardware>
|
||||
<param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
|
||||
<param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
|
||||
<var.instance.enabled.Production>true</var.instance.enabled.Production>
|
||||
<param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
|
||||
<param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
|
||||
<param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
|
||||
<param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
|
||||
<param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
|
||||
<param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
|
||||
<param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
|
||||
<param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
|
||||
<param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
|
||||
<param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
|
||||
<param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
|
||||
<param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
|
||||
<param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
|
||||
<param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
|
||||
<param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
|
||||
<param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
|
||||
<param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
|
||||
<var.instance.enabled.Target>false</var.instance.enabled.Target>
|
||||
<param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
|
||||
<param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
|
||||
<param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
|
||||
<param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
|
||||
<param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
|
||||
<param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
|
||||
<param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
|
||||
<param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
|
||||
<param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
|
||||
<param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
|
||||
<param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
|
||||
<param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
|
||||
<param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
|
||||
<param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
|
||||
<param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
|
||||
<param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
|
||||
<param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
|
||||
<param.CustomToolchainOptions />
|
||||
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
|
||||
<param.RecursionLimit>100</param.RecursionLimit>
|
||||
<param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
|
||||
<param.TargetLang>option.TargetLang.C</param.TargetLang>
|
||||
<param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
|
||||
<param.CCompilerCustomOptimizations />
|
||||
<param.GenerateMakefile>true</param.GenerateMakefile>
|
||||
<param.BuildToolEnable>false</param.BuildToolEnable>
|
||||
<param.MakeCommand>make_rtw</param.MakeCommand>
|
||||
<param.TemplateMakefile>default_tmf</param.TemplateMakefile>
|
||||
<param.BuildToolConfiguration />
|
||||
<param.InlineThreshold>10</param.InlineThreshold>
|
||||
<param.InlineThresholdMax>200</param.InlineThresholdMax>
|
||||
<param.InlineStackLimit>4000</param.InlineStackLimit>
|
||||
<param.EnableMemcpy>true</param.EnableMemcpy>
|
||||
<param.MemcpyThreshold>64</param.MemcpyThreshold>
|
||||
<param.EnableOpenMP>true</param.EnableOpenMP>
|
||||
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
|
||||
<param.PassStructByReference>false</param.PassStructByReference>
|
||||
<param.UseECoderFeatures>true</param.UseECoderFeatures>
|
||||
<unset>
|
||||
<param.WorkingFolder />
|
||||
<param.SpecifiedWorkingFolder />
|
||||
<param.SearchPaths />
|
||||
<param.SaturateOnIntegerOverflow />
|
||||
<param.PurelyIntegerCode />
|
||||
<param.DynamicMemoryAllocation />
|
||||
<param.DynamicMemoryAllocationThreshold />
|
||||
<param.MultiInstanceCode />
|
||||
<param.GenerateComments />
|
||||
<param.MATLABFcnDesc />
|
||||
<param.DataTypeReplacement />
|
||||
<param.ConvertIfToSwitch />
|
||||
<param.PreserveExternInFcnDecls />
|
||||
<param.ParenthesesLevel />
|
||||
<param.MaxIdLength />
|
||||
<param.CustomSymbolStrGlobalVar />
|
||||
<param.CustomSymbolStrType />
|
||||
<param.CustomSymbolStrField />
|
||||
<param.CustomSymbolStrFcn />
|
||||
<param.CustomSymbolStrTmpVar />
|
||||
<param.CustomSymbolStrMacro />
|
||||
<param.CustomSymbolStrEMXArray />
|
||||
<param.CustomSymbolStrEMXArrayFcn />
|
||||
<param.ReservedNameArray />
|
||||
<param.EnableScreener />
|
||||
<param.Verbose />
|
||||
<param.GenerateReport />
|
||||
<param.GenerateCodeMetricsReport />
|
||||
<param.GenerateCodeReplacementReport />
|
||||
<param.CustomInclude />
|
||||
<param.CustomSource />
|
||||
<param.CustomLibrary />
|
||||
<param.SameHardware />
|
||||
<var.instance.enabled.Production />
|
||||
<param.HardwareSizeChar.Production />
|
||||
<param.HardwareSizeShort.Production />
|
||||
<param.HardwareSizeInt.Production />
|
||||
<param.HardwareSizeLong.Production />
|
||||
<param.HardwareSizeLongLong.Production />
|
||||
<param.HardwareSizeFloat.Production />
|
||||
<param.HardwareSizeDouble.Production />
|
||||
<param.HardwareSizeWord.Production />
|
||||
<param.HardwareSizePointer.Production />
|
||||
<param.HardwareEndianness.Production />
|
||||
<param.HardwareLongLongMode.Production />
|
||||
<param.HardwareDivisionRounding.Production />
|
||||
<var.instance.enabled.Target />
|
||||
<param.HardwareSizeChar.Target />
|
||||
<param.HardwareSizeShort.Target />
|
||||
<param.HardwareSizeInt.Target />
|
||||
<param.HardwareSizeLongLong.Target />
|
||||
<param.HardwareSizeFloat.Target />
|
||||
<param.HardwareSizeDouble.Target />
|
||||
<param.HardwareEndianness.Target />
|
||||
<param.HardwareAtomicFloatSize.Target />
|
||||
<param.CustomToolchainOptions />
|
||||
<param.ConstantFoldingTimeout />
|
||||
<param.RecursionLimit />
|
||||
<param.IncludeTerminateFcn />
|
||||
<param.TargetLang />
|
||||
<param.CCompilerCustomOptimizations />
|
||||
<param.GenerateMakefile />
|
||||
<param.BuildToolEnable />
|
||||
<param.MakeCommand />
|
||||
<param.TemplateMakefile />
|
||||
<param.BuildToolConfiguration />
|
||||
<param.InlineThreshold />
|
||||
<param.InlineThresholdMax />
|
||||
<param.InlineStackLimit />
|
||||
<param.EnableMemcpy />
|
||||
<param.MemcpyThreshold />
|
||||
<param.EnableOpenMP />
|
||||
<param.InitFltsAndDblsToZero />
|
||||
<param.UseECoderFeatures />
|
||||
</unset>
|
||||
</profile>
|
||||
<param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
|
||||
<param.version>R2012a</param.version>
|
||||
<param.HasECoderFeatures>true</param.HasECoderFeatures>
|
||||
<param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
|
||||
<param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
|
||||
<param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
|
||||
<param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
|
||||
<param.SILDebugging>false</param.SILDebugging>
|
||||
<param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
|
||||
<param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
|
||||
<param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
|
||||
<param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
|
||||
<param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
|
||||
<param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
|
||||
<param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
|
||||
<param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
|
||||
<param.artifact>option.target.artifact.lib</param.artifact>
|
||||
<param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
|
||||
<param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
|
||||
<param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
|
||||
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
|
||||
<param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
|
||||
<param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
|
||||
<param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
|
||||
<param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
|
||||
<param.UsePreconditions>false</param.UsePreconditions>
|
||||
<param.FeatureFlags />
|
||||
<param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
|
||||
<param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
|
||||
<param.ComputedFixedPointData />
|
||||
<param.UserFixedPointData />
|
||||
<param.DefaultWordLength>16</param.DefaultWordLength>
|
||||
<param.DefaultFractionLength>4</param.DefaultFractionLength>
|
||||
<param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
|
||||
<param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
|
||||
<param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
|
||||
<param.StaticAnalysisTimeout />
|
||||
<param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
|
||||
<param.LogAllIOValues>false</param.LogAllIOValues>
|
||||
<param.LogHistogram>false</param.LogHistogram>
|
||||
<param.ShowCoverage>true</param.ShowCoverage>
|
||||
<param.ExcludedFixedPointVerificationFiles />
|
||||
<param.ExcludedFixedPointSimulationFiles />
|
||||
<param.InstrumentedBuildChecksum />
|
||||
<param.FixedPointStaticAnalysisChecksum />
|
||||
<param.InstrumentedMexFile />
|
||||
<param.FixedPointValidationChecksum />
|
||||
<param.FixedPointSourceCodeChecksum />
|
||||
<param.FixedPointFunctionReplacements />
|
||||
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
|
||||
<param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
|
||||
<param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
|
||||
<unset>
|
||||
<param.outputfile />
|
||||
<param.version />
|
||||
<param.HasECoderFeatures />
|
||||
<param.CallGeneratedCodeFromTest />
|
||||
<param.VerificationMode />
|
||||
<param.SILDebugging />
|
||||
<param.AutoInferUseVariableSize />
|
||||
<param.AutoInferUseUnboundedSize />
|
||||
<param.AutoInferVariableSizeThreshold />
|
||||
<param.AutoInferUnboundedSizeThreshold />
|
||||
<param.mex.outputfile />
|
||||
<param.grt.outputfile />
|
||||
<param.FixedPointTypeProposalMode />
|
||||
<param.DefaultProposedFixedPointType />
|
||||
<param.MinMaxSafetyMargin />
|
||||
<param.OptimizeWholeNumbers />
|
||||
<param.LaunchInstrumentationReport />
|
||||
<param.OpenInstrumentationReportInBrowser />
|
||||
<param.CreatePrintableInstrumentationReport />
|
||||
<param.EnableAutoExtrinsicCalls />
|
||||
<param.UsePreconditions />
|
||||
<param.FeatureFlags />
|
||||
<param.FixedPointMode />
|
||||
<param.AutoScaleLoopIndexVariables />
|
||||
<param.ComputedFixedPointData />
|
||||
<param.UserFixedPointData />
|
||||
<param.DefaultWordLength />
|
||||
<param.DefaultFractionLength />
|
||||
<param.FixedPointSafetyMargin />
|
||||
<param.FixedPointFimath />
|
||||
<param.FixedPointTypeSource />
|
||||
<param.StaticAnalysisTimeout />
|
||||
<param.StaticAnalysisGlobalRangesOnly />
|
||||
<param.LogAllIOValues />
|
||||
<param.LogHistogram />
|
||||
<param.ShowCoverage />
|
||||
<param.ExcludedFixedPointVerificationFiles />
|
||||
<param.ExcludedFixedPointSimulationFiles />
|
||||
<param.InstrumentedBuildChecksum />
|
||||
<param.FixedPointStaticAnalysisChecksum />
|
||||
<param.InstrumentedMexFile />
|
||||
<param.FixedPointValidationChecksum />
|
||||
<param.FixedPointSourceCodeChecksum />
|
||||
<param.FixedPointFunctionReplacements />
|
||||
<param.GeneratedFixedPointFileSuffix />
|
||||
<param.DefaultFixedPointSignedness />
|
||||
</unset>
|
||||
<fileset.entrypoints>
|
||||
<file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
|
||||
<Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
|
||||
<Input Name="approx_prediction">
|
||||
<Class>uint8</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="use_inertia_matrix">
|
||||
<Class>uint8</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="zFlag">
|
||||
<Class>uint8</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>3 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="dt">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="z">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>9 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="q_rotSpeed">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="q_rotAcc">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="q_acc">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="q_mag">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="r_gyro">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="r_accel">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="r_mag">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>1 x 1</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
<Input Name="J">
|
||||
<Class>single</Class>
|
||||
<UserDefined>false</UserDefined>
|
||||
<Size>3 x 3</Size>
|
||||
<Complex>false</Complex>
|
||||
</Input>
|
||||
</Inputs>
|
||||
</file>
|
||||
</fileset.entrypoints>
|
||||
<fileset.testbench>
|
||||
<file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
|
||||
</fileset.testbench>
|
||||
<fileset.package />
|
||||
<build-deliverables>
|
||||
<file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
|
||||
</build-deliverables>
|
||||
<workflow />
|
||||
<matlab>
|
||||
<root>/opt/matlab/r2013b</root>
|
||||
<toolboxes>
|
||||
<toolbox name="fixedpoint" />
|
||||
</toolboxes>
|
||||
</matlab>
|
||||
<platform>
|
||||
<unix>true</unix>
|
||||
<mac>false</mac>
|
||||
<windows>false</windows>
|
||||
<win2k>false</win2k>
|
||||
<winxp>false</winxp>
|
||||
<vista>false</vista>
|
||||
<linux>true</linux>
|
||||
<solaris>false</solaris>
|
||||
<osver>3.16.1-1-ARCH</osver>
|
||||
<os32>false</os32>
|
||||
<os64>true</os64>
|
||||
<arch>glnxa64</arch>
|
||||
<matlab>true</matlab>
|
||||
</platform>
|
||||
</configuration>
|
||||
</deployment-project>
|
||||
|
|
@ -38,6 +38,7 @@
|
|||
*
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
@ -75,8 +76,7 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#include "codegen/attitudeKalmanfilter_initialize.h"
|
||||
#include "codegen/attitudeKalmanfilter.h"
|
||||
#include "codegen/AttitudeEKF.h"
|
||||
#include "attitude_estimator_ekf_params.h"
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
@ -133,9 +133,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
|
|||
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
14000,
|
||||
7200,
|
||||
attitude_estimator_ekf_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -207,10 +207,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
float debugOutput[4] = { 0.0f };
|
||||
int overloadcounter = 19;
|
||||
|
||||
/* Initialize filter */
|
||||
attitudeKalmanfilter_initialize();
|
||||
AttitudeEKF_initialize();
|
||||
|
||||
/* store start time to guard against too slow update rates */
|
||||
uint64_t last_run = hrt_absolute_time();
|
||||
|
@ -275,9 +276,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
/* keep track of sensor updates */
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_ekf_params ekf_params {};
|
||||
struct attitude_estimator_ekf_params ekf_params;
|
||||
memset(&ekf_params, 0, sizeof(ekf_params));
|
||||
|
||||
struct attitude_estimator_ekf_param_handles ekf_param_handles {};
|
||||
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
|
||||
|
||||
/* initialize parameter handles */
|
||||
parameters_init(&ekf_param_handles);
|
||||
|
@ -528,8 +530,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
continue;
|
||||
}
|
||||
|
||||
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);
|
||||
/* Call the estimator */
|
||||
AttitudeEKF(false, // approx_prediction
|
||||
(unsigned char)ekf_params.use_moment_inertia,
|
||||
update_vect,
|
||||
dt,
|
||||
z_k,
|
||||
ekf_params.q[0], // q_rotSpeed,
|
||||
ekf_params.q[1], // q_rotAcc
|
||||
ekf_params.q[2], // q_acc
|
||||
ekf_params.q[3], // q_mag
|
||||
ekf_params.r[0], // r_gyro
|
||||
ekf_params.r[1], // r_accel
|
||||
ekf_params.r[2], // r_mag
|
||||
ekf_params.moment_inertia_J,
|
||||
x_aposteriori,
|
||||
P_aposteriori,
|
||||
Rot_matrix,
|
||||
euler,
|
||||
debugOutput);
|
||||
|
||||
/* swap values for next iteration, check for fatal inputs */
|
||||
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
|
||||
|
@ -576,6 +595,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
// compute secondary attitude
|
||||
math::Matrix<3, 3> R_adapted; //modified rotation matrix
|
||||
R_adapted = R;
|
||||
|
||||
//move z to x
|
||||
R_adapted(0, 0) = R(0, 2);
|
||||
R_adapted(1, 0) = R(1, 2);
|
||||
R_adapted(2, 0) = R(2, 2);
|
||||
//move x to z
|
||||
R_adapted(0, 2) = R(0, 0);
|
||||
R_adapted(1, 2) = R(1, 0);
|
||||
R_adapted(2, 2) = R(2, 0);
|
||||
|
||||
//change direction of pitch (convert to right handed system)
|
||||
R_adapted(0, 0) = -R_adapted(0, 0);
|
||||
R_adapted(1, 0) = -R_adapted(1, 0);
|
||||
R_adapted(2, 0) = -R_adapted(2, 0);
|
||||
math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation
|
||||
euler_angles_sec = R_adapted.to_euler();
|
||||
|
||||
att.roll_sec = euler_angles_sec(0);
|
||||
att.pitch_sec = euler_angles_sec(1);
|
||||
att.yaw_sec = euler_angles_sec(2);
|
||||
|
||||
memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec));
|
||||
|
||||
att.rollspeed_sec = -x_aposteriori[2];
|
||||
att.pitchspeed_sec = x_aposteriori[1];
|
||||
att.yawspeed_sec = x_aposteriori[0];
|
||||
att.rollacc_sec = -x_aposteriori[5];
|
||||
att.pitchacc_sec = x_aposteriori[4];
|
||||
att.yawacc_sec = x_aposteriori[3];
|
||||
|
||||
att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
|
||||
att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
|
||||
att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
|
||||
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
|
|
@ -44,28 +44,96 @@
|
|||
|
||||
/* Extended Kalman Filter covariances */
|
||||
|
||||
/* gyro process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
|
||||
/* gyro offsets process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
|
||||
|
||||
/* gyro measurement noise */
|
||||
/**
|
||||
* Body angular rate process noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
|
||||
|
||||
/**
|
||||
* Body angular acceleration process noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
|
||||
|
||||
/**
|
||||
* Acceleration process noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
|
||||
|
||||
/**
|
||||
* Magnet field vector process noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
||||
|
||||
/**
|
||||
* Gyro measurement noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
|
||||
/* accel measurement noise */
|
||||
|
||||
/**
|
||||
* Accel measurement noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
|
||||
/* mag measurement noise */
|
||||
|
||||
/**
|
||||
* Mag measurement noise
|
||||
*
|
||||
* @group attitude_ekf
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
|
||||
/* offset estimation - UNUSED */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
|
||||
|
||||
/* magnetic declination, in degrees */
|
||||
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
|
||||
|
||||
PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (1, 1)
|
||||
*
|
||||
* @group attitude_ekf
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (2, 2)
|
||||
*
|
||||
* @group attitude_ekf
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (3, 3)
|
||||
*
|
||||
* @group attitude_ekf
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
|
||||
|
||||
/**
|
||||
* Moment of inertia enabled in estimator
|
||||
*
|
||||
* If set to != 0 the moment of inertia will be used in the estimator
|
||||
*
|
||||
* @group attitude_ekf
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_J_EN, 0);
|
||||
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
|
@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
|||
h->q1 = param_find("EKF_ATT_V3_Q1");
|
||||
h->q2 = param_find("EKF_ATT_V3_Q2");
|
||||
h->q3 = param_find("EKF_ATT_V3_Q3");
|
||||
h->q4 = param_find("EKF_ATT_V3_Q4");
|
||||
|
||||
h->r0 = param_find("EKF_ATT_V4_R0");
|
||||
h->r1 = param_find("EKF_ATT_V4_R1");
|
||||
h->r2 = param_find("EKF_ATT_V4_R2");
|
||||
h->r3 = param_find("EKF_ATT_V4_R3");
|
||||
|
||||
h->mag_decl = param_find("ATT_MAG_DECL");
|
||||
|
||||
h->acc_comp = param_find("ATT_ACC_COMP");
|
||||
|
||||
h->moment_inertia_J[0] = param_find("ATT_J11");
|
||||
h->moment_inertia_J[1] = param_find("ATT_J22");
|
||||
h->moment_inertia_J[2] = param_find("ATT_J33");
|
||||
h->use_moment_inertia = param_find("ATT_J_EN");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
|
|||
param_get(h->q1, &(p->q[1]));
|
||||
param_get(h->q2, &(p->q[2]));
|
||||
param_get(h->q3, &(p->q[3]));
|
||||
param_get(h->q4, &(p->q[4]));
|
||||
|
||||
param_get(h->r0, &(p->r[0]));
|
||||
param_get(h->r1, &(p->r[1]));
|
||||
param_get(h->r2, &(p->r[2]));
|
||||
param_get(h->r3, &(p->r[3]));
|
||||
|
||||
param_get(h->mag_decl, &(p->mag_decl));
|
||||
p->mag_decl *= M_PI_F / 180.0f;
|
||||
|
||||
param_get(h->acc_comp, &(p->acc_comp));
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
|
||||
}
|
||||
param_get(h->use_moment_inertia, &(p->use_moment_inertia));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
|
@ -42,8 +42,10 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_ekf_params {
|
||||
float r[9];
|
||||
float q[12];
|
||||
float r[3];
|
||||
float q[4];
|
||||
float moment_inertia_J[9];
|
||||
int32_t use_moment_inertia;
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
|
@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
|
|||
};
|
||||
|
||||
struct attitude_estimator_ekf_param_handles {
|
||||
param_t r0, r1, r2, r3;
|
||||
param_t q0, q1, q2, q3, q4;
|
||||
param_t r0, r1, r2;
|
||||
param_t q0, q1, q2, q3;
|
||||
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
|
||||
param_t use_moment_inertia;
|
||||
param_t mag_decl;
|
||||
param_t acc_comp;
|
||||
};
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,26 @@
|
|||
/*
|
||||
* AttitudeEKF.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Thu Aug 21 11:17:28 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEEKF_H__
|
||||
#define __ATTITUDEEKF_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "AttitudeEKF_types.h"
|
||||
|
||||
/* Function Declarations */
|
||||
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
|
||||
extern void AttitudeEKF_initialize(void);
|
||||
extern void AttitudeEKF_terminate(void);
|
||||
#endif
|
||||
/* End of code generation (AttitudeEKF.h) */
|
|
@ -0,0 +1,17 @@
|
|||
/*
|
||||
* AttitudeEKF_types.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Thu Aug 21 11:17:28 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEEKF_TYPES_H__
|
||||
#define __ATTITUDEEKF_TYPES_H__
|
||||
|
||||
/* Include files */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
#endif
|
||||
/* End of code generation (AttitudeEKF_types.h) */
|
File diff suppressed because it is too large
Load Diff
|
@ -1,34 +0,0 @@
|
|||
/*
|
||||
* attitudeKalmanfilter.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEKALMANFILTER_H__
|
||||
#define __ATTITUDEKALMANFILTER_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter.h) */
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue