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

Merged master into indoor branch

This commit is contained in:
Lorenz Meier 2014-12-26 17:06:19 +01:00
parent f3a224e30d 5c51adf5f7
commit 407889ea2c
229 changed files with 27468 additions and 4247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -38,3 +38,4 @@ tags
.pydevproject
.ropeproject
*.orig
Firmware.zip

5
.gitmodules vendored
View File

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

95
.travis.yml Normal file
View File

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

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

@ -1 +1 @@
Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f
Subproject commit 3c36467c0d5572431a09ae50013328a4693ee070

29
README.md
View File

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

4
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
View File

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

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

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

4
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
View File

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

31
ROMFS/px4fmu_common/init.d/10018_tbs_endurance Normal file
View File

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

2
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
View File

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

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

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

2
ROMFS/px4fmu_common/init.d/12001_octo_cox
View File

@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_cox
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678

16
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol Normal file
View File

@ -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
ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
View File

@ -2,4 +2,4 @@
sh /etc/init.d/rc.fw_defaults
set MIXER FMU_AERT
set MIXER skywalker

2
ROMFS/px4fmu_common/init.d/3030_io_camflyer
View File

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

4
ROMFS/px4fmu_common/init.d/3031_phantom
View File

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

2
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

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

4
ROMFS/px4fmu_common/init.d/3033_wingwing
View File

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

4
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
View File

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

2
ROMFS/px4fmu_common/init.d/4001_quad_x
View File

@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_OUT 1234

4
ROMFS/px4fmu_common/init.d/4008_ardrone
View File

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

2
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

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

2
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

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

2
ROMFS/px4fmu_common/init.d/4012_quad_x_can
View File

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

2
ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
View File

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

2
ROMFS/px4fmu_common/init.d/5001_quad_+
View File

@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
set PWM_OUT 1234

2
ROMFS/px4fmu_common/init.d/6001_hexa_x
View File

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

2
ROMFS/px4fmu_common/init.d/7001_hexa_+
View File

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

2
ROMFS/px4fmu_common/init.d/8001_octo_x
View File

@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_x
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678

2
ROMFS/px4fmu_common/init.d/9001_octo_+
View File

@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_+
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678

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

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

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

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

31
ROMFS/px4fmu_common/init.d/rc.interface
View File

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

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

@ -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
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

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

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

@ -71,6 +71,10 @@ if px4flow start
then
fi
if ll40ls start
then
fi
#
# Start sensors -> preflight_check
#

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

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

15
ROMFS/px4fmu_common/init.d/rc.vtol_apps Normal file
View File

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

39
ROMFS/px4fmu_common/init.d/rc.vtol_defaults Normal file
View File

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

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

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

16
ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix Normal file
View File

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

64
ROMFS/px4fmu_common/mixers/skywalker.mix Normal file
View File

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

4
ROMFS/px4fmu_test/init.d/rc.standalone
View File

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

10
ROMFS/px4fmu_test/init.d/rcS
View File

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

2
Tools/px4params/srcparser.py
View File

@ -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):
"""

7
Tools/px4params/srcscanner.py
View File

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

54
Tools/tests-host/Makefile
View File

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

28
Tools/tests-host/autodeclination_test.cpp
View File

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

2
makefiles/config_px4fmu-v1_default.mk
View File

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

6
makefiles/config_px4fmu-v2_default.mk
View File

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

17
makefiles/config_px4fmu-v2_test.mk
View File

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

10
makefiles/gumstix-aerocore.cfg Normal file
View File

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

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

@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
#
ARCHWARNINGS = -Wall \
-Wextra \
-Werror \
-Wdouble-promotion \
-Wshadow \
-Wfloat-equal \

5
makefiles/upload.mk
View File

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

3
nuttx-configs/px4fmu-v1/nsh/Make.defs
View File

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

3
nuttx-configs/px4fmu-v2/nsh/Make.defs
View File

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

3
nuttx-configs/px4io-v1/nsh/Make.defs
View File

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

3
nuttx-configs/px4io-v2/nsh/Make.defs
View File

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

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

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

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

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

3
src/drivers/boards/aerocore/aerocore_led.c
View File

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

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

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

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

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

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

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

2
src/drivers/hott/hott_sensors/hott_sensors.cpp
View File

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

2
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
View File

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

22
src/drivers/ll40ls/ll40ls.cpp
View File

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

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

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

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

@ -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'");
}

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

@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow
SRCS = px4flow.cpp
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wno-attributes

343
src/drivers/px4flow/px4flow.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -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);
}

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

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

14
src/drivers/rgbled/rgbled.cpp
View File

@ -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>");

2
src/drivers/roboclaw/roboclaw_main.cpp
View File

@ -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")) {

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

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

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

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

13
src/examples/flow_speed_control/module.mk → src/drivers/trone/module.mk
View File

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

915
src/drivers/trone/trone.cpp Normal file
View File

@ -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'");
}

155
src/examples/fixedwing_control/main.c
View File

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

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

@ -41,3 +41,5 @@ SRCS = main.c \
params.c
MODULE_STACKSIZE = 1200
EXTRACFLAGS = -Wframe-larger-than=1200

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

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

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

@ -39,3 +39,5 @@ MODULE_COMMAND = flow_position_estimator
SRCS = flow_position_estimator_main.c \
flow_position_estimator_params.c
EXTRACFLAGS = -Wno-float-equal

387
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -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(&param_handles);
parameters_update(&param_handles, &params);
/* 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(&param_handles, &params);
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;
}

100
src/examples/hwtest/hwtest.c
View File

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

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

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

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

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

5
src/lib/conversion/rotation.cpp
View File

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

2
src/lib/rc/st24.c
View File

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

1
src/lib/uavcan Submodule

@ -0,0 +1 @@
Subproject commit 1efd24427539fa332a15151143466ec760fa5fff

298
src/modules/attitude_estimator_ekf/AttitudeEKF.m Normal file
View File

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

502
src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj Normal file
View File

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

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

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

106
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

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

12
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
View File

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

1456
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c Normal file
View File

File diff suppressed because it is too large Load Diff

26
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h Normal file
View File

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

17
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h Normal file
View File

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

1148
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
View File

File diff suppressed because it is too large Load Diff

34
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
View File

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