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

updated to master (solved merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-27 09:26:27 +01:00
parent 560efcbc5b 4241e526aa
commit dd59409e31
289 changed files with 10555 additions and 4461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -55,3 +55,5 @@ src/platforms/posix-arm/px4_messages/
src/platforms/qurt/px4_messages/
ROMFS/*/*/rc.autostart
rootfs/
*.autosave
CMakeLists.txt.user
.gitmodules
+9 -6
View File
@@ -13,12 +13,15 @@
[submodule "Tools/gencpp"]
path = Tools/gencpp
url = https://github.com/ros/gencpp.git
[submodule "unittests/gtest"]
path = unittests/gtest
url = https://github.com/sjwilks/gtest.git
[submodule "src/lib/eigen"]
path = src/lib/eigen
url = https://github.com/PX4/eigen.git
[submodule "src/lib/dspal"]
path = src/lib/dspal
url = https://github.com/mcharleb/dspal.git
[submodule "Tools/jMAVSim"]
path = Tools/jMAVSim
url = https://github.com/PX4/jMAVSim.git
[submodule "Tools/sitl_gazebo"]
path = Tools/sitl_gazebo
url = https://github.com/PX4/sitl_gazebo.git
[submodule "unittests/googletest"]
path = unittests/googletest
url = https://github.com/google/googletest.git
.travis.yml
+31 -49
View File
@@ -4,11 +4,13 @@
language: cpp
matrix:
fast_finish: true
include:
- os: linux
sudo: false
- os: osx
osx_image: beta-xcode6.3
sudo: true
cache:
directories:
@@ -19,16 +21,18 @@ addons:
sources:
- kubuntu-backports
- ubuntu-toolchain-r-test
- george-edison55-precise-backports
packages:
- build-essential
- ccache
- cmake
- clang-3.5
- cmake
- g++-4.8
- gcc-4.8
- genromfs
- libc6-i386
- libncurses5-dev
- ninja-build
- python-argparse
- python-empy
- python-serial
@@ -54,10 +58,11 @@ before_install:
elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
brew tap PX4/homebrew-px4
&& brew update
&& brew install astyle
&& brew install gcc-arm-none-eabi
&& brew install genromfs
&& brew install kconfig-frontends
&& brew install gcc-arm-none-eabi
&& brew install astyle
&& brew install ninja
&& sudo easy_install pip
&& sudo pip install pyserial empy
;
@@ -80,6 +85,7 @@ before_script:
env:
global:
- NINJA_BUILD=1
# AWS KEY: $PX4_AWS_KEY
- secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA="
# AWS SECRET: $PX4_AWS_SECRET
@@ -89,53 +95,29 @@ env:
script:
- make check_format
- arm-none-eabi-gcc --version
- echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r'
- make posix_sitl_simple
- echo -en 'travis_fold:end:script.1\\r'
- echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r'
- make posix_sitl_simple test
- cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h
- echo -en 'travis_fold:end:script.2\\r'
- echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.3\\r'
- make px4fmu-v1_default
- make px4fmu-v2_default
- echo -en 'travis_fold:end:script.3\\r'
- echo 'Running Tests..' && echo -en 'travis_fold:start:script.4\\r'
- make px4fmu-v2_default test
- echo -en 'travis_fold:end:script.4\\r'
#- make px4fmu-v2_default package
#- make posix -j4
#- ccache -s
#- echo -en 'travis_fold:end:script.1\\r'
#- echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r'
#- make tests
#- cat src/modules/systemlib/mixer/mixer_multirotor.generated.h
#- echo -en 'travis_fold:end:script.2\\r'
#- echo 'Building NuttX..' && echo -en 'travis_fold:start:script.3\\r'
#- make archives
#- ccache -s
#- echo -en 'travis_fold:end:script.3\\r'
#- echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.4\\r'
#- make -j4
#- make size
#- ccache -s
#- echo -en 'travis_fold:end:script.4\\r'
#- zip Firmware.zip Images/*.px4
- echo 'Building POSIX Firmware..' && make posix_sitl_simple
- echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h
- echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
- echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
- echo 'Running Tests..' && make px4fmu-v2_default test
#after_script:
#- git clone git://github.com/PX4/CI-Tools.git
#- ./CI-Tools/s3cmd-configure
## upload newest build for this branch with s3 index
#- ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
## archive newest build by date with s3 index
#- ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
#- ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
## upload top level index.html and timestamp.html
#- ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
#- ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
#- echo ""
#- echo "Binaries have been posted to:"
#- echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
after_success:
- if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4
&& cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4
&& zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
&& git clone git://github.com/PX4/CI-Tools.git
&& ./CI-Tools/s3cmd-configure
&& ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
&& ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
&& ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
&& echo ""
&& echo "Binaries have been posted to:"
&& echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
;
fi
deploy:
provider: releases
CMakeLists.txt
+19 -2
View File
@@ -117,6 +117,8 @@
#
#=============================================================================
# Warning: Changing this modifies CMake's internal workings
# and leads to wrong toolchain detection
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
#=============================================================================
@@ -184,6 +186,7 @@ project(px4 CXX C ASM)
if (NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0)
cmake_policy(SET CMP0045 NEW) # error on non-existent target in get prop
cmake_policy(SET CMP0046 NEW) # no non-existent targets as dependencies
cmake_policy(SET CMP0025 OLD) # still report AppleClang as Clang
endif()
if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0)
cmake_policy(SET CMP0054 NEW) # don't dereference quoted variables
@@ -225,9 +228,10 @@ px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen")
px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
add_custom_target(submodule_clean
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
@@ -235,6 +239,19 @@ add_custom_target(submodule_clean
COMMAND rm -rf .git/modules/*
)
#=============================================================================
# misc targets
#
add_custom_target(check_format
COMMAND Tools/check_code_style.sh
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
)
add_custom_target(config
COMMAND cmake-gui .
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
)
#=============================================================================
# external libraries
#
@@ -274,7 +291,7 @@ px4_generate_messages(TARGET msg_gen
px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD})
px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD})
add_custom_target(xml_gen
DEPENDS git_eigen parameters.xml airframes.xml)
DEPENDS parameters.xml airframes.xml)
#=============================================================================
# external projects
Makefile
+36 -39
View File
@@ -36,7 +36,19 @@
# We depend on our submodules, so we have to prevent attempts to
# compile without it being present.
ifeq ($(wildcard .git),)
$(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.)
$(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.)
endif
CMAKE_VER := $(shell Tools/check_cmake.sh; echo $$?)
ifneq ($(CMAKE_VER),0)
$(warning Not a valid CMake version or CMake not installed.)
$(warning On Ubuntu, install or upgrade via:)
$(warning )
$(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y)
$(warning sudo apt-get update)
$(warning sudo apt-get install cmake)
$(warning )
$(error Fatal)
endif
# Help
@@ -67,12 +79,11 @@ all: px4fmu-v2_default
ARGS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS))
j ?= 4
# disable ninja by default for now because it hides upload progress
#NINJA_BUILD := $(shell ninja --version 2>/dev/null)
NINJA_BUILD := $(shell ninja --version 2>/dev/null)
ifdef NINJA_BUILD
PX4_CMAKE_GENERATOR ?= "Ninja"
PX4_MAKE = ninja
PX4_MAKE_ARGS =
PX4_MAKE_ARGS =
else
ifdef SYSTEMROOT
@@ -89,6 +100,7 @@ endif
# --------------------------------------------------------------------
# describe how to build a cmake config
define cmake-build
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
+$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
endef
@@ -113,57 +125,40 @@ px4fmu-v2_default:
px4fmu-v2_simple:
$(call cmake-build,nuttx_px4fmu-v2_simple)
px4fmu-v2_lpe:
$(call cmake-build,nuttx_px4fmu-v2_lpe)
nuttx_sim_simple:
$(call cmake-build,$@)
posix_sitl_simple:
$(call cmake-build,$@)
posix_sitl_lpe:
$(call cmake-build,$@)
ros_sitl_simple:
$(call cmake-build,$@)
qurt_eagle_travis:
$(call cmake-build,$@)
posix_eagle_release:
$(call cmake-build,$@)
posix: posix_sitl_simple
posix_sitl_default: posix_sitl_simple
ros: ros_sitl_simple
run_sitl_quad: posix
Tools/sitl_run.sh posix-configs/SITL/init/rcS
sitl_deprecation:
@echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead."
@echo "Change init script with 'make posix_sitl_default config'"
run_sitl_plane: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing
run_sitl_ros: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros
lldb_sitl_quad: posix
Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb
lldb_sitl_plane: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb
lldb_sitl_ros: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb
gdb_sitl_quad: posix
Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb
gdb_sitl_plane: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb
gdb_sitl_ros: posix
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb
sitl_quad:
@echo "Deprecated. Use 'run_sitl_quad' instead."
sitl_plane:
@echo "Deprecated. Use 'run_sitl_plane' instead."
sitl_ros:
@echo "Deprecated. Use 'run_sitl_ros' instead."
sitl_quad: sitl_deprecation
sitl_plane: sitl_deprecation
sitl_ros: sitl_deprecation
# Other targets
# --------------------------------------------------------------------
@@ -176,7 +171,9 @@ clean:
@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
jmavsim_gdb jmavsim_lldb
$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
.PHONY: clean
README.md
+2 -2
View File
@@ -4,7 +4,7 @@
[![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)
This repository contains the PX4 Flight Core, with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones.
This repository contains the [PX4 Flight Core](http://px4.io), with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones.
* Official Website: http://px4.io
* License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md))
@@ -32,7 +32,7 @@ Software in the Loop guide:
Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl).
Developer guide:
http://px4.io/dev/
http://dev.px4.io
Testing guide:
http://px4.io/dev/unit_tests
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
-8
View File
@@ -28,11 +28,3 @@ fi
set MIXER quad_w
set PWM_OUT 1234
set PWM_MIN 1200
set MIXER_AUX pass
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
ROMFS/px4fmu_common/init.d/10016_3dr_iris
-3
View File
@@ -32,6 +32,3 @@ fi
set MIXER quad_w
set PWM_OUT 1234
set PWM_MIN 1200
set PWM_MAX 1950
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
-3
View File
@@ -30,7 +30,4 @@ fi
set MIXER quad_w
set PWM_MIN 1210
set PWM_MAX 2100
set PWM_OUT 1234
ROMFS/px4fmu_common/init.d/10019_sk450_deadcat
-1
View File
@@ -30,7 +30,6 @@ fi
set MIXER sk450_deadcat
set PWM_OUT 1234
set PWM_MIN 1050
set PWM_AUX_OUT 1234
# set PWM_AUX_MIN 900
ROMFS/px4fmu_common/init.d/4011_dji_f450
+2 -3
View File
@@ -15,13 +15,12 @@ sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.16
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.01
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.16
param set MC_PITCHRATE_P 0.15
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.01
param set MC_YAW_P 2.8
ROMFS/px4fmu_common/init.d/rc.mc_apps
+7 -1
View File
@@ -12,7 +12,13 @@ then
attitude_estimator_q start
position_estimator_inav start
else
ekf_att_pos_estimator start
if param compare LPE_ENABLED 1
then
attitude_estimator_q start
local_position_estimator start
else
ekf_att_pos_estimator start
fi
fi
if mc_att_control start
ROMFS/px4fmu_common/init.d/rcS
+3 -3
View File
@@ -260,11 +260,11 @@ then
#
# Check if PX4IO present and update firmware if needed
#
if [ -f /etc/extras/px4io-v2_default.bin ]
if [ -f /etc/extras/px4io-v2.bin ]
then
set IO_FILE /etc/extras/px4io-v2_default.bin
set IO_FILE /etc/extras/px4io-v2.bin
else
set IO_FILE /etc/extras/px4io-v1_default.bin
set IO_FILE /etc/extras/px4io-v1.bin
fi
if px4io checkcrc ${IO_FILE}
ROMFS/px4fmu_test/init.d/rcS
+3 -3
View File
@@ -55,11 +55,11 @@ else
echo "[param] FAILED loading $PARAM_FILE"
fi
if [ -f /etc/extras/px4io-v2_default.bin ]
if [ -f /etc/extras/px4io-v2.bin ]
then
set io_file /etc/extras/px4io-v2_default.bin
set io_file /etc/extras/px4io-v2.bin
else
set io_file /etc/extras/px4io-v1_default.bin
set io_file /etc/extras/px4io-v1.bin
fi
if px4io start
Tools/check_cmake.sh
Executable
+14
View File
@@ -0,0 +1,14 @@
#!/bin/bash
cmake_ver=`cmake --version`
if [[ $cmake_ver == "" ]]
then
exit 1;
fi
if [[ $cmake_ver == *"2.8"* ]] || [[ $cmake_ver == *"2.9"* ]] || [[ $cmake_ver == *"3.0"* ]] || [[ $cmake_ver == *"3.1"* ]]
then
exit 1;
fi
exit 0;
Tools/check_code_style.sh
+19 -9
View File
@@ -4,22 +4,32 @@ failed=0
for fn in $(find src/examples \
src/systemcmds \
src/include \
src/drivers/blinkm \
src/drivers/bma180 \
src/drivers/pca9685 \
src/drivers/pca8574 \
src/drivers/md25 \
src/drivers/ms5611 \
src/drivers/stm32 \
src/drivers/px4io \
src/drivers/px4fmu \
src/drivers \
src/platforms \
src/firmware \
src/lib/launchdetection \
src/lib/geo \
src/lib/geo_lookup \
src/lib/conversion \
src/lib/rc \
src/lib/version \
src/modules/attitude_estimator_q \
src/modules/gpio_led \
src/modules/land_detector \
src/modules/muorb \
src/modules/px4iofirmware \
src/modules/param \
src/modules/sensors \
src/modules/simulator \
src/modules/uORB \
src/modules/bottle_drop \
src/modules/dataman \
src/modules/fixedwing_backside \
src/modules/segway \
src/modules/local_position_estimator \
src/modules/unit_test \
src/modules/systemlib \
src/modules/controllib \
-path './Build' -prune -o \
-path './mavlink' -prune -o \
-path './NuttX' -prune -o \
Tools/generate_listener.py
+3 -3
View File
@@ -119,8 +119,8 @@ print("""
#define PRIu64 "llu"
#endif
#ifndef PRI64
#define PRI64 "lld"
#ifndef PRId64
#define PRId64 "lld"
#endif
""")
@@ -184,7 +184,7 @@ for index,m in enumerate(messages[1:]):
print("\t\t\t}")
print("\t\t\tprintf(\"\\n\");")
elif item[0] == "int64":
print("\t\t\tprintf(\"%s: %%\" PRI64 \"\\n\",container.%s);" % (item[1], item[1]))
print("\t\t\tprintf(\"%s: %%\" PRId64 \"\\n\",container.%s);" % (item[1], item[1]))
elif item[0] == "int32":
print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1]))
elif item[0] == "uint32":
Tools/jMAVSim
Submodule
+1
Submodule Tools/jMAVSim added at 96029523d1
Tools/posix.gdbinit
+1 -1
View File
@@ -1,2 +1,2 @@
handle SIGCONT nostop
handle SIGCONT nostop noprint nopass
run
Tools/px4params/srcparser.py
+2 -1
View File
@@ -45,6 +45,7 @@ class Parameter(object):
"min": 5,
"max": 4,
"unit": 3,
"decimal": 2,
# all others == 0 (sorted alphabetically)
}
@@ -106,7 +107,7 @@ class SourceParser(object):
re_remove_dots = re.compile(r'\.+$')
re_remove_carriage_return = re.compile('\n+')
valid_tags = set(["group", "board", "min", "max", "unit"])
valid_tags = set(["group", "board", "min", "max", "unit", "decimal"])
# Order of parameter groups
priority = {
Tools/sitl_gazebo
Submodule
+1
Submodule Tools/sitl_gazebo added at d362e661b4
Tools/sitl_run.sh
+77 -8
View File
@@ -1,16 +1,85 @@
#!/bin/bash
cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit
cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit
cd build_posix_sitl_simple/src/firmware/posix
rc_script=$1
debugger=$2
program=$3
build_path=$4
curr_dir=`pwd`
echo SITL ARGS
echo rc_script: $rc_script
echo debugger: $debugger
echo program: $program
echo build_path: $build_path
if [ "$#" != 4 ]
then
echo usage: sitl_run.sh rc_script debugger program build_path
exit 1
fi
# kill process names that might stil
# be running from last time
pkill gazebo
pkill mainapp
jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
if [ -n "$jmavsim_pid" ]
then
kill $jmavsim_pid
fi
cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
SIM_PID=0
if [ "$program" == "jmavsim" ]
then
cd Tools/jMAVSim
ant
java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
SIM_PID=`echo $!`
elif [ "$3" == "gazebo" ]
then
if [ -x "$(command -v gazebo)" ]
then
# Set the plugin path so Gazebo finds our model and sim
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build
# Set the model path so Gazebo finds the airframes
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
# Disable online model lookup since this is quite experimental and unstable
export GAZEBO_MODEL_DATABASE_URI=""
export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
mkdir -p Tools/sitl_gazebo/Build
cd Tools/sitl_gazebo/Build
cmake ..
make -j4
gazebo ../worlds/iris.world &
SIM_PID=`echo $!`
else
echo "You need to have gazebo simulator installed!"
exit 1
fi
fi
cd $build_path/src/firmware/posix
mkdir -p rootfs/fs/microsd
mkdir -p rootfs/eeprom
touch rootfs/eeprom/parameters
if [ "$2" == "lldb" ]
# Start Java simulator
if [ "$debugger" == "lldb" ]
then
lldb -- mainapp ../../../../$1
elif [ "$2" == "gdb" ]
lldb -- mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "gdb" ]
then
gdb --args mainapp ../../../../$1
gdb --args mainapp ../../../../${rc_script}_${program}
else
./mainapp ../../../../$1
./mainapp ../../../../${rc_script}_${program}
fi
if [ "$3" == "jmavsim" ]
then
kill -9 $SIM_PID
elif [ "$3" == "gazebo" ]
then
kill -9 $SIM_PID
fi
Vagrantfile
Vendored
+7 -3
View File
@@ -26,7 +26,7 @@ Vagrant.configure(2) do |config|
# Create a private network, which allows host-only access to the machine
# using a specific IP.
# config.vm.network "private_network", ip: "192.168.33.10"
config.vm.network "private_network", ip: "192.168.33.10"
# Create a public network, which generally matched to bridged network.
# Bridged networks make the machine appear as another physical device on
@@ -37,7 +37,7 @@ Vagrant.configure(2) do |config|
# the path on the host to the actual folder. The second argument is
# the path on the guest to mount the folder. And the optional third
# argument is a set of non-required options.
config.vm.synced_folder ".", "/Firmware"
config.vm.synced_folder ".", "/Firmware", type: "nfs"
# Provider-specific configuration so you can fine-tune various
# backing providers for Vagrant. These expose provider-specific options.
@@ -77,14 +77,18 @@ Vagrant.configure(2) do |config|
# Ensure we start in the Firmware folder
echo "cd /Firmware" >> ~/.bashrc
# Install software
sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded -y
sudo add-apt-repository ppa:george-edison55/cmake-3.x -y
sudo apt-get update
sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb
sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf
pushd .
cd ~
wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2
tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2
exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH"
if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
exportline2="export HEXAGON_TOOLS_ROOT=$HOME/Qualcomm/HEXAGON_Tools/7.2.10/Tools"
if grep -Fxq "$exportline2" ~/.profile; then echo nothing to do ; else echo $exportline2 >> ~/.profile; fi
. ~/.profile
popd
# setup ccache
cmake/common/px4_base.cmake
+6 -5
View File
@@ -433,6 +433,7 @@ function(px4_add_upload)
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
COMMENT "uploading ${BUNDLE}"
VERBATIM
USES_TERMINAL
)
endfunction()
@@ -492,7 +493,6 @@ function(px4_add_common_flags)
-Wpointer-arith
-Wmissing-declarations
-Wno-unused-parameter
-Wno-varargs
-Werror=format-security
-Werror=array-bounds
-Wfatal-errors
@@ -512,11 +512,12 @@ function(px4_add_common_flags)
list(APPEND warnings -Wframe-larger-than=1024)
endif()
if (${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
if (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
# QuRT 6.4.X compiler identifies as Clang but does not support this option
if (NOT ${OS} STREQUAL "qurt")
list(APPEND warnings
-Wno-unused-const-variable
-Wno-varargs
)
endif()
else()
@@ -539,7 +540,7 @@ function(px4_add_common_flags)
-fdata-sections
)
if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
list(APPEND optimization_flags
-fno-strength-reduce
-fno-builtin-printf
@@ -553,7 +554,7 @@ function(px4_add_common_flags)
-Wnested-externs
)
if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
list(APPEND c_warnings
-Wold-style-declaration
-Wmissing-parameter-type
@@ -632,7 +633,7 @@ function(px4_add_common_flags)
-DCONFIG_ARCH_BOARD_${board_config}
)
if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
set(added_exe_link_flags
-Wl,--warn-common
-Wl,--gc-sections
cmake/configs/nuttx_px4fmu-v2_default.cmake
+1
View File
@@ -22,6 +22,7 @@ set(config_module_list
drivers/hmc5883
drivers/ms5611
drivers/mb12xx
drivers/srf02
drivers/sf0x
drivers/ll40ls
drivers/trone
cmake/configs/nuttx_px4fmu-v2_lpe.cmake
+5
View File
@@ -0,0 +1,5 @@
include(cmake/configs/nuttx_px4fmu-v2_default.cmake)
list(APPEND config_module_list
modules/local_position_estimator
)
cmake/configs/posix_eagle_release.cmake
+9
View File
@@ -5,6 +5,15 @@ if("${DSPAL_STUBS_ENABLE}" STREQUAL "")
set(DSPAL_STUBS_ENABLE "1")
endif()
if ("${QRL_SDK_DIR}" STREQUAL "")
set(QRL_SDK_DIR /opt/qrlsdk)
endif()
set(CMAKE_PROGRAM_PATH
"${QRL_SDK_DIR}/gcc-linaro-4.8-2015.06-x86_64_arm-linux-gnueabihf/bin"
${CMAKE_PROGRAM_PATH}
)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
set(config_module_list
cmake/configs/posix_sitl_lpe.cmake
+9
View File
@@ -0,0 +1,9 @@
include(cmake/configs/posix_sitl_simple.cmake)
list(APPEND config_module_list
modules/local_position_estimator
)
set(config_sitl_rcS
posix-configs/SITL/init/rcS_lpe
)
cmake/configs/posix_sitl_simple.cmake
+21
View File
@@ -62,6 +62,27 @@ set(config_extra_builtin_cmds
sercon
)
set(config_sitl_rcS
posix-configs/SITL/init/rcS
CACHE FILEPATH "init script for sitl"
)
set(config_sitl_viewer
jmavsim
CACHE STRING "viewer for sitl"
)
set_property(CACHE config_sitl_viewer
PROPERTY STRINGS "jmavsim;none")
set(config_sitl_debugger
disable
CACHE STRING "debugger for sitl"
)
set_property(CACHE config_sitl_debugger
PROPERTY STRINGS "disable;gdb;lldb")
add_custom_target(sercon)
set_target_properties(sercon PROPERTIES
MAIN "sercon" STACK "2048")
cmake/configs/qurt_eagle_hello.cmake
+1 -1
View File
@@ -1,6 +1,6 @@
include(qurt/px4_impl_qurt)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
drivers/device
cmake/configs/qurt_eagle_hil.cmake
+1 -1
View File
@@ -1,6 +1,6 @@
include(qurt/px4_impl_qurt)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
drivers/device
cmake/configs/qurt_eagle_muorb.cmake
+1 -1
View File
@@ -1,6 +1,6 @@
include(qurt/px4_impl_qurt)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
drivers/device
cmake/configs/qurt_eagle_release.cmake
+1 -1
View File
@@ -20,7 +20,7 @@ set(target_libraries
)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
#
cmake/configs/qurt_eagle_test.cmake
+1 -1
View File
@@ -1,6 +1,6 @@
include(qurt/px4_impl_qurt)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
drivers/device
cmake/configs/qurt_eagle_travis.cmake
+1 -1
View File
@@ -3,7 +3,7 @@ include(qurt/px4_impl_qurt)
# Run a full link with build stubs to make sure qurt target isn't broken
set(QURT_ENABLE_STUBS "1")
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
set(config_module_list
drivers/device
cmake/qurt/px4_impl_qurt.cmake
+1 -8
View File
@@ -222,14 +222,7 @@ function(px4_os_prebuild_targets)
ONE_VALUE OUT BOARD THREADS
REQUIRED OUT BOARD
ARGN ${ARGN})
add_custom_target(git_eigen_patched
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/src/lib/eigen
COMMAND git checkout .
COMMAND git checkout master
COMMAND patch -p1 -i ${CMAKE_SOURCE_DIR}/cmake/qurt/qurt_eigen.patch
DEPENDS git_eigen)
add_custom_target(${OUT} DEPENDS git_dspal git_eigen_patched)
add_custom_target(ALL DEPENDS git_eigen_patched)
add_custom_target(${OUT} DEPENDS git_dspal git_eigen)
endfunction()
cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
+3 -3
View File
@@ -26,13 +26,13 @@ set(CMAKE_SYSTEM_VERSION 1)
# specify the cross compiler
find_program(C_COMPILER arm-linux-gnueabihf-gcc)
if(NOT C_COMPILER)
message(FATAL_ERROR "could not find arm-none-eabi-gcc compiler")
message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler")
endif()
cmake_force_c_compiler(${C_COMPILER} GNU)
find_program(CXX_COMPILER arm-linux-gnueabihf-g++)
if(NOT CXX_COMPILER)
message(FATAL_ERROR "could not find arm-none-eabi-g++ compiler")
message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler")
endif()
cmake_force_cxx_compiler(${CXX_COMPILER} GNU)
@@ -41,7 +41,7 @@ foreach(tool objcopy nm ld)
string(TOUPPER ${tool} TOOL)
find_program(${TOOL} arm-linux-gnueabihf-${tool})
if(NOT ${TOOL})
message(FATAL_ERROR "could not find ${tool}")
message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}")
endif()
endforeach()
cmake/toolchains/Toolchain-hexagon-7.4.cmake → cmake/toolchains/Toolchain-hexagon-7.2.10.cmake
+17 -3
View File
@@ -34,8 +34,12 @@ include(CMakeForceCompiler)
list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake)
include(common/px4_base)
if(NOT HEXAGON_TOOLS_ROOT)
set(HEXAGON_TOOLS_ROOT /opt/7.4/Tools)
if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "")
message(FATAL_ERROR
"The HexagonTools version 7.2.10 must be installed and the environment variable HEXAGON_TOOLS_ROOT must be set"
"(e.g. export HEXAGON_TOOLS_ROOT=/opt/HEXAGON_Tools/7.2.10/Tools)")
else()
set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT})
endif()
macro (list2string out in)
@@ -53,7 +57,7 @@ set(HEXAGON_LIB_DIR ${HEXAGON_TOOLS_ROOT}/gnu/hexagon/lib)
set(HEXAGON_ISS_DIR ${HEXAGON_TOOLS_ROOT}/lib/iss)
set(TOOLSLIB ${HEXAGON_TOOLS_ROOT}/target/hexagon/lib/${V_ARCH}/G0)
# Use the HexagonTools compiler (6.4.05)
# Use the HexagonTools compiler (7.2.10)
set(CMAKE_C_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang)
set(CMAKE_CXX_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang++)
@@ -82,6 +86,7 @@ set(ARCHCPUFLAGS
add_definitions(
-D_PID_T -D_UID_T -D_TIMER_T
-Dnoreturn_function=
-D_HAS_C9X
-D__EXPORT=
-Drestrict=
-D_DEBUG
@@ -238,3 +243,12 @@ set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
# for libraries and headers in the target directories
set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
# The Hexagon compiler doesn't support the -rdynamic flag and this is set
# in the base cmake scripts. We have to redefine the __linux_compiler_gnu
# macro for cmake 2.8 to work
set(__LINUX_COMPILER_GNU 1)
macro(__linux_compiler_gnu lang)
set(CMAKE_SHARED_LIBRARY_LINK_${lang}_FLAGS "")
endmacro()
cmake/toolchains/Toolchain-posix-clang-native.cmake
-90
View File
@@ -1,90 +0,0 @@
set(WARNINGS
-Wall
-Werror
-Wextra
-Wdouble-promotion
-Wshadow
-Wfloat-equal
-Wframe-larger-than=1024
-Wpointer-arith
-Wlogical-op
-Wmissing-declarations
-Wno-unused-parameter
-Werror=format-security
-Werror=array-bounds
-Wfatal-errors
-Wformat=1
-Werror=unused-but-set-variable
-Werror=unused-variable
-Werror=double-promotion
-Werror=reorder
-Werror=uninitialized
-Werror=init-self
-Werror=return-type
-Werror=deprecated
-Werror=unused-private-field
-Wno-packed
-Wno-frame-larger-than=
-Wno-varargs
#-Wcast-qual - generates spurious noreturn attribute warnings,
# try again later
#-Wconversion - would be nice, but too many "risky-but-safe"
# conversions in the code
#-Wcast-align - would help catch bad casts in some cases,
# but generates too many false positives
)
set(OPT_FLAGS
-Os -g3
)
#=============================================================================
# c flags
#
set(C_WARNINGS
-Wbad-function-cast
-Wstrict-prototypes
-Wold-style-declaration
-Wmissing-parameter-type
-Wmissing-prototypes
-Wnested-externs
)
set(C_FLAGS
-std=gnu99
-fno-common
)
#=============================================================================
# cxx flags
#
set(CXX_WARNINGS
-Wno-missing-field-initializers
-Wno-varargs
)
set(CXX_FLAGS
-fno-exceptions
-fno-rtti
-std=gnu++0x
-fno-threadsafe-statics
-DCONFIG_WCHAR_BUILTIN
-D__CUSTOM_FILE_IO__
)
#=============================================================================
# ld flags
#
set(LD_FLAGS
-Wl,--warn-common
-Wl,--gc-sections
)
#=============================================================================
# misc flags
#
set(VISIBILITY_FLAGS
-fvisibility=hidden
"-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h"
)
set(EXE_LINK_FLAGS)
nuttx-configs/px4fmu-v1/nsh/Make.defs
-3
View File
@@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \
-Wextra \
-Wdouble-promotion \
-Wshadow \
-Wfloat-equal \
-Wframe-larger-than=1024 \
-Wpointer-arith \
-Wlogical-op \
-Wmissing-declarations \
-Wpacked \
-Wno-unused-parameter
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
@@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wstrict-prototypes \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs
ARCHWARNINGSXX = $(ARCHWARNINGS) \
-Wno-psabi
nuttx-configs/px4fmu-v2/nsh/Make.defs
-3
View File
@@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \
-Wextra \
-Wdouble-promotion \
-Wshadow \
-Wfloat-equal \
-Wframe-larger-than=1024 \
-Wpointer-arith \
-Wlogical-op \
-Wmissing-declarations \
-Wpacked \
-Wno-unused-parameter
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
@@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wstrict-prototypes \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs
ARCHWARNINGSXX = $(ARCHWARNINGS) \
-Wno-psabi
nuttx-configs/px4io-v1/nsh/Make.defs
-3
View File
@@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \
-Wextra \
-Wdouble-promotion \
-Wshadow \
-Wfloat-equal \
-Wframe-larger-than=1024 \
-Wpointer-arith \
-Wlogical-op \
-Wmissing-declarations \
-Wpacked \
-Wno-unused-parameter
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
@@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wstrict-prototypes \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs
ARCHWARNINGSXX = $(ARCHWARNINGS)
ARCHDEFINES =
nuttx-configs/px4io-v2/nsh/Make.defs
-3
View File
@@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \
-Wextra \
-Wdouble-promotion \
-Wshadow \
-Wfloat-equal \
-Wframe-larger-than=1024 \
-Wpointer-arith \
-Wlogical-op \
-Wmissing-declarations \
-Wpacked \
-Wno-unused-parameter
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
@@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wstrict-prototypes \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs
ARCHWARNINGSXX = $(ARCHWARNINGS)
ARCHDEFINES =
posix-configs/SITL/init/rcS_gazebo
+59
View File
@@ -0,0 +1,59 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1376256
param set CAL_ACC1_ID 1310720
param set CAL_MAG0_ID 196608
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set COM_RC_IN_MODE 2
rgbled start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
posix-configs/SITL/init/rcS → posix-configs/SITL/init/rcS_jmavsim
View File
posix-configs/SITL/init/rcS_lpe_gazebo
+66
View File
@@ -0,0 +1,66 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1376256
param set CAL_ACC1_ID 1310720
param set CAL_MAG0_ID 196608
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 1
param set CAL_ACC0_YOFF 2
param set CAL_ACC0_ZOFF 3
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
# changes for LPE
param set COM_RC_IN_MODE 1
param set LPE_BETA_MAX 10000
rgbled start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
posix-configs/SITL/init/rcS_lpe_jmavsim
+65
View File
@@ -0,0 +1,65 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.15
param set MC_ROLLRATE_P 0.15
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1376256
param set CAL_ACC1_ID 1310720
param set CAL_MAG0_ID 196608
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.4
param set MPC_XY_VEL_P 0.2
param set MPC_XY_VEL_D 0.005
param set SENS_BOARD_ROT 0
# changes for LPE
param set COM_RC_IN_MODE 1
param set LPE_BETA_MAX 10000
rgbled start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
mavlink start -u 14556 -r 2000000
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
src/drivers/airspeed/airspeed.cpp
+1
View File
@@ -133,6 +133,7 @@ Airspeed::init()
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr) {
goto out;
}
src/drivers/ardrone_interface/ardrone_interface.c
+30 -21
View File
@@ -92,8 +92,10 @@ static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
if (reason) {
warnx("%s\n", reason);
}
warnx("usage: {start|stop|status} [-d <UART>]\n\n");
exit(1);
}
@@ -122,11 +124,11 @@ int ardrone_interface_main(int argc, char *argv[])
thread_should_exit = false;
ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
1100,
ardrone_interface_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
1100,
ardrone_interface_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
@@ -138,9 +140,11 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
} else {
warnx("not started");
}
exit(0);
}
@@ -212,19 +216,23 @@ int ardrone_interface_thread_main(int argc, char *argv[])
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
if (i+1 < argc) {
int motor = atoi(argv[i+1]);
if (i + 1 < argc) {
int motor = atoi(argv[i + 1]);
if (motor > 0 && motor < 5) {
test_motor = motor;
} else {
thread_running = false;
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
}
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
if (argc > i + 1) {
device = argv[i + 1];
@@ -314,6 +322,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int motors[4] = {0, 0, 0, 0};
motors[test_motor - 1] = 10;
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
} else {
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
}
@@ -338,33 +347,33 @@ int ardrone_interface_thread_main(int argc, char *argv[])
}
if (counter % 24 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 0) { ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); }
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 1) { ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); }
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (led_counter == 2) { ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); }
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (led_counter == 3) { ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); }
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (led_counter == 4) { ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); }
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (led_counter == 5) { ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); }
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (led_counter == 6) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); }
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (led_counter == 7) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); }
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (led_counter == 8) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); }
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (led_counter == 9) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); }
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (led_counter == 10) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); }
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
if (led_counter == 11) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); }
led_counter++;
if (led_counter == 12) led_counter = 0;
if (led_counter == 12) { led_counter = 0; }
}
/* run at approximately 200 Hz */
src/drivers/ardrone_interface/ardrone_motor_control.c
+32 -22
View File
@@ -108,7 +108,7 @@ void ar_enable_broadcast(int fd)
int ar_multiplexing_init()
{
int fd;
fd = open(PX4FMU_DEVICE_PATH, 0);
if (fd < 0) {
@@ -176,7 +176,7 @@ int ar_select_motor(int fd, uint8_t motor)
ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
} else {
/* select reqested motor */
/* select reqested motor */
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
}
@@ -199,7 +199,7 @@ int ar_deselect_motor(int fd, uint8_t motor)
ret += ioctl(fd, GPIO_SET, motor_gpios);
} else {
/* deselect reqested motor */
/* deselect reqested motor */
ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]);
}
@@ -235,7 +235,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
*/
write(ardrone_uart, &(initbuf[0]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
usleep(UART_TRANSFER_TIME_BYTE_US * 1);
/*
* write 0x91 - request checksum
@@ -243,7 +243,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
*/
write(ardrone_uart, &(initbuf[1]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*120);
usleep(UART_TRANSFER_TIME_BYTE_US * 120);
/*
* write 0xA1 - set status OK
@@ -252,7 +252,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
*/
write(ardrone_uart, &(initbuf[2]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
usleep(UART_TRANSFER_TIME_BYTE_US * 1);
/*
* set as motor i, where i = 1..4
@@ -268,7 +268,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
*/
write(ardrone_uart, &(initbuf[4]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*11);
usleep(UART_TRANSFER_TIME_BYTE_US * 11);
ar_deselect_motor(gpios, i);
/* sleep 200 ms */
@@ -304,13 +304,15 @@ int ar_init_motors(int ardrone_uart, int gpios)
warnx("Failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
}
/**
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green,
uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
{
/*
* 2 bytes are sent. The first 3 bits describe the command: 011 means led control
@@ -322,12 +324,15 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
* 011 rrrr 0000 gggg 0
*/
uint8_t leds[2];
leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((
led1_red & 0x01) << 1);
leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((
led1_green & 0x01) << 1);
write(ardrone_uart, leds, 2);
}
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
{
const unsigned int min_motor_interval = 4900;
static uint64_t last_motor_time = 0;
@@ -338,6 +343,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
outputs.output[2] = motor3;
outputs.output[3] = motor4;
static orb_advert_t pub = 0;
if (pub == 0) {
/* advertise to channel 0 / primary */
pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
@@ -355,15 +361,18 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
if (ret == sizeof(buf)) {
return OK;
} else {
return ret;
}
} else {
return -ERROR;
}
}
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators)
{
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
@@ -385,7 +394,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
output_band = motor_thrust / startpoint_full_control; // linear from 0 to 1
} else {
output_band = 1.0f;
}
@@ -407,7 +417,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
/* if one motor is saturated, reduce throttle */
float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]), fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
if (saturation > 0.0f) {
@@ -428,16 +438,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
/* set the motor values */
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[0] = (uint16_t)(motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t)(motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t)(motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t)(motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[0] = (uint16_t)(motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[1] = (uint16_t)(motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[2] = (uint16_t)(motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[3] = (uint16_t)(motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
/* Failsafe logic for min values - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
src/drivers/boards/aerocore/aerocore_init.c
+9 -3
View File
@@ -117,12 +117,12 @@ __END_DECLS
static GRAN_HANDLE dma_allocator;
/*
* The DMA heap size constrains the total number of things that can be
/*
* The DMA heap size constrains the total number of things that can be
* ready to do DMA at a time.
*
* For example, FAT DMA depends on one sector-sized buffer per filesystem plus
* one sector-sized buffer per file.
* one sector-sized buffer per file.
*
* We use a fundamental alignment / granule size of 64B; this is sufficient
* to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
@@ -137,8 +137,10 @@ dma_alloc_init(void)
sizeof(g_dma_heap),
7, /* 128B granule - must be > alignment (XXX bug?) */
6); /* 64B alignment */
if (dma_allocator == NULL) {
message("[boot] DMA allocator setup FAILED");
} else {
g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
}
@@ -262,11 +264,13 @@ __EXPORT int nsh_archinitialize(void)
/* Configure Sensors on SPI bus #3 */
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default: 1MHz, 8 bits, Mode 3 */
SPI_SETFREQUENCY(spi3, 10000000);
SPI_SETBITS(spi3, 8);
@@ -279,11 +283,13 @@ __EXPORT int nsh_archinitialize(void)
/* Configure FRAM on SPI bus #4 */
spi4 = up_spiinitialize(4);
if (!spi4) {
message("[boot] FAILED to initialize SPI port 4\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default: ~10MHz, 8 bits, Mode 3 */
SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
SPI_SETBITS(spi4, 8);
src/drivers/boards/aerocore/aerocore_led.c
+10 -4
View File
@@ -103,17 +103,23 @@ __EXPORT void led_toggle(int led)
{
switch (led) {
case 0:
if (stm32_gpioread(GPIO_LED0))
if (stm32_gpioread(GPIO_LED0)) {
stm32_gpiowrite(GPIO_LED0, false);
else
} else {
stm32_gpiowrite(GPIO_LED0, true);
}
break;
case 1:
if (stm32_gpioread(GPIO_LED1))
if (stm32_gpioread(GPIO_LED1)) {
stm32_gpiowrite(GPIO_LED1, false);
else
} else {
stm32_gpiowrite(GPIO_LED1, true);
}
break;
default:
src/drivers/boards/aerocore/aerocore_spi.c
+2 -2
View File
@@ -136,8 +136,8 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
break;
src/drivers/boards/px4-stm32f4discovery/board_config.h
+5 -5
View File
@@ -54,7 +54,7 @@ __BEGIN_DECLS
#include <arch/board/board.h>
#define UDID_START 0x1FFF7A10
/****************************************************************************************************
* Definitions
****************************************************************************************************/
@@ -66,13 +66,13 @@ __BEGIN_DECLS
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
/* SPI chip selects */
#define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
+7 -8
View File
@@ -69,8 +69,7 @@ __EXPORT void led_init()
__EXPORT void led_on(int led)
{
if (led == 1)
{
if (led == 1) {
/* Pull down to switch on */
stm32_gpiowrite(GPIO_LED1, false);
}
@@ -78,8 +77,7 @@ __EXPORT void led_on(int led)
__EXPORT void led_off(int led)
{
if (led == 1)
{
if (led == 1) {
/* Pull up to switch off */
stm32_gpiowrite(GPIO_LED1, true);
}
@@ -87,11 +85,12 @@ __EXPORT void led_off(int led)
__EXPORT void led_toggle(int led)
{
if (led == 1)
{
if (stm32_gpioread(GPIO_LED1))
if (led == 1) {
if (stm32_gpioread(GPIO_LED1)) {
stm32_gpiowrite(GPIO_LED1, false);
else
} else {
stm32_gpiowrite(GPIO_LED1, true);
}
}
}
src/drivers/boards/px4fmu-v1/px4fmu_init.c
+16 -16
View File
@@ -223,23 +223,23 @@ __EXPORT int nsh_archinitialize(void)
* Keep the SPI2 init optional and conditionally initialize the ADC pins
*/
#ifdef CONFIG_STM32_SPI2
spi2 = up_spiinitialize(2);
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
#ifdef CONFIG_STM32_SPI2
spi2 = up_spiinitialize(2);
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
#else
spi2 = NULL;
message("[boot] Enabling IN12/13 instead of SPI2\n");
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
#endif
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
#else
spi2 = NULL;
message("[boot] Enabling IN12/13 instead of SPI2\n");
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
#endif
/* Get the SPI port for the microSD slot */
src/drivers/boards/px4fmu-v1/px4fmu_led.c
+17 -16
View File
@@ -70,13 +70,12 @@ __EXPORT void led_init(void)
__EXPORT void led_on(int led)
{
if (led == 0)
{
if (led == 0) {
/* Pull down to switch on */
stm32_gpiowrite(GPIO_LED1, false);
}
if (led == 1)
{
if (led == 1) {
/* Pull down to switch on */
stm32_gpiowrite(GPIO_LED2, false);
}
@@ -84,13 +83,12 @@ __EXPORT void led_on(int led)
__EXPORT void led_off(int led)
{
if (led == 0)
{
if (led == 0) {
/* Pull up to switch off */
stm32_gpiowrite(GPIO_LED1, true);
}
if (led == 1)
{
if (led == 1) {
/* Pull up to switch off */
stm32_gpiowrite(GPIO_LED2, true);
}
@@ -98,18 +96,21 @@ __EXPORT void led_off(int led)
__EXPORT void led_toggle(int led)
{
if (led == 0)
{
if (stm32_gpioread(GPIO_LED1))
if (led == 0) {
if (stm32_gpioread(GPIO_LED1)) {
stm32_gpiowrite(GPIO_LED1, false);
else
} else {
stm32_gpiowrite(GPIO_LED1, true);
}
}
if (led == 1)
{
if (stm32_gpioread(GPIO_LED2))
if (led == 1) {
if (stm32_gpioread(GPIO_LED2)) {
stm32_gpiowrite(GPIO_LED2, false);
else
} else {
stm32_gpiowrite(GPIO_LED2, true);
}
}
}
src/drivers/boards/px4fmu-v1/px4fmu_spi.c
+2 -2
View File
@@ -90,8 +90,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
break;
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+11 -7
View File
@@ -118,12 +118,12 @@ __END_DECLS
static GRAN_HANDLE dma_allocator;
/*
* The DMA heap size constrains the total number of things that can be
/*
* The DMA heap size constrains the total number of things that can be
* ready to do DMA at a time.
*
* For example, FAT DMA depends on one sector-sized buffer per filesystem plus
* one sector-sized buffer per file.
* one sector-sized buffer per file.
*
* We use a fundamental alignment / granule size of 64B; this is sufficient
* to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
@@ -138,8 +138,10 @@ dma_alloc_init(void)
sizeof(g_dma_heap),
7, /* 128B granule - must be > alignment (XXX bug?) */
6); /* 64B alignment */
if (dma_allocator == NULL) {
message("[boot] DMA allocator setup FAILED");
} else {
g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
}
@@ -214,12 +216,12 @@ static struct sdio_dev_s *sdio;
/*#ifdef __cplusplus*/
/*__EXPORT int matherr(struct __exception *e)*/
/*{*/
/*return 1;*/
/*return 1;*/
/*}*/
/*#else*/
/*__EXPORT int matherr(struct exception *e)*/
/*{*/
/*return 1;*/
/*return 1;*/
/*}*/
/*#endif*/
@@ -326,10 +328,11 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
#ifdef CONFIG_MMCSD
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
@@ -338,6 +341,7 @@ __EXPORT int nsh_archinitialize(void)
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
@@ -346,7 +350,7 @@ __EXPORT int nsh_archinitialize(void)
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
#endif
#endif
return OK;
}
src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+7 -8
View File
@@ -69,8 +69,7 @@ __EXPORT void led_init()
__EXPORT void led_on(int led)
{
if (led == 1)
{
if (led == 1) {
/* Pull down to switch on */
stm32_gpiowrite(GPIO_LED1, false);
}
@@ -78,8 +77,7 @@ __EXPORT void led_on(int led)
__EXPORT void led_off(int led)
{
if (led == 1)
{
if (led == 1) {
/* Pull up to switch off */
stm32_gpiowrite(GPIO_LED1, true);
}
@@ -87,11 +85,12 @@ __EXPORT void led_off(int led)
__EXPORT void led_toggle(int led)
{
if (led == 1)
{
if (stm32_gpioread(GPIO_LED1))
if (led == 1) {
if (stm32_gpioread(GPIO_LED1)) {
stm32_gpiowrite(GPIO_LED1, false);
else
} else {
stm32_gpiowrite(GPIO_LED1, true);
}
}
}
src/drivers/boards/px4io-v1/board_config.h
+6 -6
View File
@@ -30,10 +30,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file board_config.h
*
*
* PX4IO hardware definitions.
*/
@@ -59,11 +59,11 @@
/* LEDs */
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* Safety switch button *************************************************************/
@@ -86,7 +86,7 @@
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
/*
/*
* High-resolution timer
*/
#define HRT_TIMER 1 /* use timer1 for the HRT */
src/drivers/camera_trigger/camera_trigger.cpp
+8 -6
View File
@@ -195,6 +195,7 @@ CameraTrigger::CameraTrigger() :
// Convert number to individual channels
unsigned i = 0;
int single_pin;
while ((single_pin = pin_list % 10)) {
_pins[i] = single_pin - 1;
@@ -226,18 +227,19 @@ CameraTrigger::control(bool on)
if (on) {
// schedule trigger on and off calls
hrt_call_every(&_engagecall, 500, (_interval * 1000),
(hrt_callout)&CameraTrigger::engage, this);
(hrt_callout)&CameraTrigger::engage, this);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
(hrt_callout)&CameraTrigger::disengage, this);
} else {
// cancel all calls
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
// ensure that the pin is off
hrt_call_after(&_disengagecall, 500,
(hrt_callout)&CameraTrigger::disengage, this);
(hrt_callout)&CameraTrigger::disengage, this);
}
_trigger_enabled = on;
@@ -249,7 +251,7 @@ CameraTrigger::start()
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
stm32_configgpio(_gpios[_pins[i]]);
stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
}
// enable immediate if configured that way
@@ -316,7 +318,7 @@ CameraTrigger::cycle_trampoline(void *arg)
}
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
void
@@ -361,7 +363,7 @@ CameraTrigger::info()
{
warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
_polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
_polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
warnx("interval : %.2f", (double)_interval);
}
src/drivers/device/cdev.cpp
+30 -14
View File
@@ -100,14 +100,16 @@ CDev::CDev(const char *name,
_registered(false),
_open_count(0)
{
for (unsigned i = 0; i < _max_pollwaiters; i++)
for (unsigned i = 0; i < _max_pollwaiters; i++) {
_pollset[i] = nullptr;
}
}
CDev::~CDev()
{
if (_registered)
if (_registered) {
unregister_driver(_devname);
}
}
int
@@ -124,13 +126,16 @@ CDev::register_class_devname(const char *class_devname)
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
if (ret == OK) { break; }
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
@@ -148,15 +153,17 @@ CDev::init()
// base class init first
int ret = Device::init();
if (ret != OK)
if (ret != OK) {
goto out;
}
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
if (ret != OK) {
goto out;
}
_registered = true;
}
@@ -182,8 +189,9 @@ CDev::open(file_t *filp)
/* first-open callback may decline the open */
ret = open_first(filp);
if (ret != OK)
if (ret != OK) {
_open_count--;
}
}
unlock();
@@ -209,8 +217,9 @@ CDev::close(file_t *filp)
_open_count--;
/* callback cannot decline the close */
if (_open_count == 0)
if (_open_count == 0) {
ret = close_last(filp);
}
} else {
ret = -EBADF;
@@ -250,26 +259,30 @@ CDev::ioctl(file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
/* fetch a pointer to the driver's private data */
/* fetch a pointer to the driver's private data */
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
return OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
return OK;
break;
case DEVIOCGPUBBLOCK:
return _pub_blocked;
break;
}
/* try the superclass. The different ioctl() function form
* means we need to copy arg */
unsigned arg2 = arg;
* means we need to copy arg */
unsigned arg2 = arg;
int ret = Device::ioctl(cmd, arg2);
if (ret != -ENODEV)
if (ret != -ENODEV) {
return ret;
}
return -ENOTTY;
}
@@ -305,8 +318,9 @@ CDev::poll(file_t *filp, struct pollfd *fds, bool setup)
fds->revents |= fds->events & poll_state(filp);
/* yes? post the notification */
if (fds->revents != 0)
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
}
} else {
@@ -328,8 +342,9 @@ CDev::poll_notify(pollevent_t events)
irqstate_t state = irqsave();
for (unsigned i = 0; i < _max_pollwaiters; i++)
if (nullptr != _pollset[i])
if (nullptr != _pollset[i]) {
poll_notify_one(_pollset[i], events);
}
irqrestore(state);
}
@@ -342,8 +357,9 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events)
/* if the state is now interesting, wake the waiter if it's still asleep */
/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
if ((fds->revents != 0) && (fds->sem->semcount <= 0))
if ((fds->revents != 0) && (fds->sem->semcount <= 0)) {
px4_sem_post(fds->sem);
}
}
pollevent_t
src/drivers/device/device_nuttx.cpp
+10 -5
View File
@@ -95,7 +95,7 @@ Device::Device(const char *name,
_irq_attached(false)
{
sem_init(&_lock, 0, 1);
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
@@ -109,8 +109,9 @@ Device::~Device()
{
sem_destroy(&_lock);
if (_irq_attached)
if (_irq_attached) {
unregister_interrupt(_irq);
}
}
int
@@ -126,8 +127,9 @@ Device::init()
/* register */
ret = register_interrupt(_irq, this);
if (ret != OK)
if (ret != OK) {
goto out;
}
_irq_attached = true;
}
@@ -139,15 +141,17 @@ out:
void
Device::interrupt_enable()
{
if (_irq_attached)
if (_irq_attached) {
up_enable_irq(_irq);
}
}
void
Device::interrupt_disable()
{
if (_irq_attached)
if (_irq_attached) {
up_disable_irq(_irq);
}
}
void
@@ -224,6 +228,7 @@ Device::ioctl(unsigned operation, unsigned &arg)
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
}
src/drivers/device/device_nuttx.h
+25 -20
View File
@@ -118,7 +118,7 @@ public:
* @param data The buffer from which values should be read.
* @param count The number of items to write.
* @return The number of items written on success, negative errno otherwise.
*/
*/
virtual int write(unsigned address, void *data, unsigned count);
/**
@@ -147,13 +147,13 @@ public:
parameter protocol without loss of information.
*/
struct DeviceStructure {
enum DeviceBusType bus_type:3;
uint8_t bus:5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
enum DeviceBusType bus_type : 3;
uint8_t bus: 5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
union DeviceId {
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
@@ -189,14 +189,16 @@ protected:
*
* Note that we must loop as the wait may be interrupted by a signal.
*/
void lock() {
void lock()
{
do {} while (sem_wait(&_lock) != 0);
}
/**
* Release the driver lock.
*/
void unlock() {
void unlock()
{
sem_post(&_lock);
}
@@ -416,7 +418,7 @@ protected:
*/
virtual int close_last(file_t *filp);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -425,7 +427,7 @@ protected:
*/
virtual int register_class_devname(const char *class_devname);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -440,7 +442,7 @@ protected:
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
const char *get_devname() { return _devname; }
bool _pub_blocked; /**< true if publishing should be blocked */
@@ -470,8 +472,8 @@ private:
int remove_poll_waiter(struct pollfd *fds);
/* do not allow copying this class */
CDev(const CDev&);
CDev operator=(const CDev&);
CDev(const CDev &);
CDev operator=(const CDev &);
};
/**
@@ -503,7 +505,8 @@ protected:
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
uint32_t reg(uint32_t offset)
{
return *(volatile uint32_t *)(_base + offset);
}
@@ -513,7 +516,8 @@ protected:
* @param offset Register offset in bytes from the base address.
* @param value Value to write.
*/
void reg(uint32_t offset, uint32_t value) {
void reg(uint32_t offset, uint32_t value)
{
*(volatile uint32_t *)(_base + offset) = value;
}
@@ -527,7 +531,8 @@ protected:
* @param clearbits Bits to clear in the register
* @param setbits Bits to set in the register
*/
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) {
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits)
{
uint32_t val = reg(offset);
val &= ~clearbits;
val |= setbits;
@@ -542,9 +547,9 @@ private:
// class instance for primary driver of each class
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
CLASS_DEVICE_PRIMARY = 0,
CLASS_DEVICE_SECONDARY = 1,
CLASS_DEVICE_TERTIARY = 2
};
#endif /* _DEVICE_DEVICE_H */
src/drivers/device/device_posix.cpp
+9 -8
View File
@@ -60,7 +60,7 @@ Device::Device(const char *name) :
if (ret != 0) {
PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno));
}
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
@@ -86,23 +86,24 @@ Device::init()
int
Device::dev_read(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
return -ENODEV;
}
int
Device::dev_write(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
return -ENODEV;
}
int
Device::dev_ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
switch (operation) {
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
}
} // namespace device
src/drivers/device/i2c_nuttx.cpp
+18 -9
View File
@@ -69,7 +69,7 @@ I2C::I2C(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
I2C::~I2C()
@@ -92,6 +92,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
if (_bus_clocks[index] > 0) {
// DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
}
_bus_clocks[index] = clock_hz;
return OK;
@@ -122,7 +123,7 @@ I2C::init()
(void)up_i2cuninitialize(_dev);
_dev = nullptr;
DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
ret = -EINVAL;
goto out;
}
@@ -162,13 +163,15 @@ I2C::init()
// tell the world where we are
DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
out:
if ((ret != OK) && (_dev != nullptr)) {
up_i2cuninitialize(_dev);
_dev = nullptr;
}
return ret;
}
@@ -208,18 +211,21 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
msgs++;
}
if (msgs == 0)
if (msgs == 0) {
return -EINVAL;
}
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
/* success */
if (ret == OK)
if (ret == OK) {
break;
}
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
if ((retry_count >= 1) || (retry_count >= _retries)) {
up_i2creset(_dev);
}
} while (retry_count++ < _retries);
@@ -234,20 +240,23 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
unsigned retry_count = 0;
/* force the device address into the message vector */
for (unsigned i = 0; i < msgs; i++)
for (unsigned i = 0; i < msgs; i++) {
msgv[i].addr = _address;
}
do {
ret = I2C_TRANSFER(_dev, msgv, msgs);
/* success */
if (ret == OK)
if (ret == OK) {
break;
}
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
if ((retry_count >= 1) || (retry_count >= _retries)) {
up_i2creset(_dev);
}
} while (retry_count++ < _retries);
src/drivers/device/i2c_nuttx.h
+2 -1
View File
@@ -134,7 +134,8 @@ protected:
*
* @param address The new bus address to set.
*/
void set_address(uint16_t address) {
void set_address(uint16_t address)
{
_address = address;
_device_id.devid_s.address = _address;
}
src/drivers/device/i2c_posix.cpp
+40 -28
View File
@@ -75,7 +75,7 @@ I2C::I2C(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
I2C::~I2C()
@@ -105,6 +105,7 @@ I2C::init()
}
_fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY);
if (_fd < 0) {
DEVICE_DEBUG("px4_open failed of device %s", get_devname());
return PX4_ERROR;
@@ -116,16 +117,18 @@ I2C::init()
if (simulate) {
_fd = 10000;
}
else {
} else {
#ifndef __PX4_QURT
// Open the actual I2C device and map to the virtual dev name
_fd = ::open(get_devname(), O_RDWR);
if (_fd < 0) {
warnx("could not open %s", get_devname());
px4_errno = errno;
return PX4_ERROR;
}
#endif
}
@@ -135,9 +138,9 @@ I2C::init()
int
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
#ifndef __PX4_LINUX
#ifndef __PX4_LINUX
return 1;
#else
#else
struct i2c_msg msgv[2];
unsigned msgs;
struct i2c_rdwr_ioctl_data packets;
@@ -145,7 +148,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
unsigned retry_count = 0;
if (_fd < 0) {
warnx("I2C device not opened");
warnx("I2C device not opened");
return 1;
}
@@ -169,8 +172,9 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
msgs++;
}
if (msgs == 0)
if (msgs == 0) {
return -EINVAL;
}
packets.msgs = msgv;
packets.nmsgs = msgs;
@@ -178,9 +182,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
if (simulate) {
//warnx("I2C SIM: transfer_4 on %s", get_devname());
ret = PX4_OK;
}
else {
} else {
ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets);
if (ret < 0) {
warnx("I2C transfer failed");
return 1;
@@ -188,28 +193,30 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
}
/* success */
if (ret == PX4_OK)
if (ret == PX4_OK) {
break;
}
} while (retry_count++ < _retries);
return ret;
#endif
#endif
}
int
I2C::transfer(struct i2c_msg *msgv, unsigned msgs)
{
#ifndef __PX4_LINUX
#ifndef __PX4_LINUX
return 1;
#else
#else
struct i2c_rdwr_ioctl_data packets;
int ret;
unsigned retry_count = 0;
/* force the device address into the message vector */
for (unsigned i = 0; i < msgs; i++)
for (unsigned i = 0; i < msgs; i++) {
msgv[i].addr = _address;
}
do {
packets.msgs = msgv;
@@ -218,34 +225,38 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs)
if (simulate) {
warnx("I2C SIM: transfer_2 on %s", get_devname());
ret = PX4_OK;
}
else {
} else {
ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets);
}
if (ret < 0) {
warnx("I2C transfer failed");
return 1;
}
warnx("I2C transfer failed");
return 1;
}
/* success */
if (ret == PX4_OK)
if (ret == PX4_OK) {
break;
}
} while (retry_count++ < _retries);
return ret;
#endif
#endif
}
int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
//struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg;
switch (cmd) {
#ifdef __PX4_LINUX
#ifdef __PX4_LINUX
case I2C_RDWR:
warnx("Use I2C::transfer, not ioctl");
warnx("Use I2C::transfer, not ioctl");
return 0;
#endif
#endif
default:
/* give it to the superclass */
return VDev::ioctl(filp, cmd, arg);
@@ -256,27 +267,28 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen)
{
if (simulate) {
// FIXME no idea what this should be
warnx ("2C SIM I2C::read");
warnx("2C SIM I2C::read");
return 0;
}
#ifndef __PX4_QURT
return ::read(_fd, buffer, buflen);
#else
return 0;
return 0;
#endif
}
ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen)
{
if (simulate) {
warnx ("2C SIM I2C::write");
warnx("2C SIM I2C::write");
return buflen;
}
#ifndef __PX4_QURT
return ::write(_fd, buffer, buflen);
#else
return buflen;
return buflen;
#endif
}
src/drivers/device/i2c_posix.h
+2 -2
View File
@@ -127,8 +127,8 @@ private:
int _fd;
std::string _dname;
I2C(const device::I2C&);
I2C operator=(const device::I2C&);
I2C(const device::I2C &);
I2C operator=(const device::I2C &);
};
} // namespace device
src/drivers/device/integrator.cpp
+2 -1
View File
@@ -82,7 +82,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
// following:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
}
@@ -117,6 +117,7 @@ math::Vector<3>
Integrator::read(bool auto_reset)
{
math::Vector<3> val = _integral_read;
if (auto_reset) {
_integral_read(0) = 0.0f;
_integral_read(1) = 0.0f;
src/drivers/device/integrator.h
+4 -3
View File
@@ -43,7 +43,8 @@
#include <mathlib/mathlib.h>
class Integrator {
class Integrator
{
public:
Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
virtual ~Integrator();
@@ -91,6 +92,6 @@ private:
bool _coning_comp_on; /**< coning compensation */
/* we don't want this class to be copied */
Integrator(const Integrator&);
Integrator operator=(const Integrator&);
Integrator(const Integrator &);
Integrator operator=(const Integrator &);
};
src/drivers/device/ringbuffer.cpp
+25 -10
View File
@@ -46,15 +46,16 @@ namespace ringbuffer
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
_num_items(num_items),
_item_size(item_size),
_buf(new char[(_num_items+1) * item_size]),
_head(_num_items),
_tail(_num_items)
_buf(new char[(_num_items + 1) * item_size]),
_head(_num_items),
_tail(_num_items)
{}
RingBuffer::~RingBuffer()
{
if (_buf != nullptr)
if (_buf != nullptr) {
delete[] _buf;
}
}
unsigned
@@ -84,20 +85,25 @@ RingBuffer::size()
void
RingBuffer::flush()
{
while (!empty())
while (!empty()) {
get(NULL);
}
}
bool
RingBuffer::put(const void *val, size_t val_size)
{
unsigned next = _next(_head);
if (next != _tail) {
if ((val_size == 0) || (val_size > _item_size))
if ((val_size == 0) || (val_size > _item_size)) {
val_size = _item_size;
}
memcpy(&_buf[_head * _item_size], val, val_size);
_head = next;
return true;
} else {
return false;
}
@@ -169,11 +175,14 @@ RingBuffer::force(const void *val, size_t val_size)
bool overwrote = false;
for (;;) {
if (put(val, val_size))
if (put(val, val_size)) {
break;
}
get(NULL);
overwrote = true;
}
return overwrote;
}
@@ -246,6 +255,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned
*a = c;
return true;
}
return false;
}
@@ -259,8 +269,9 @@ RingBuffer::get(void *val, size_t val_size)
unsigned candidate;
unsigned next;
if ((val_size == 0) || (val_size > _item_size))
if ((val_size == 0) || (val_size > _item_size)) {
val_size = _item_size;
}
do {
/* decide which element we think we're going to read */
@@ -270,13 +281,15 @@ RingBuffer::get(void *val, size_t val_size)
next = _next(candidate);
/* go ahead and read from this index */
if (val != NULL)
if (val != NULL) {
memcpy(val, &_buf[candidate * _item_size], val_size);
}
/* if the tail pointer didn't change, we got our item */
} while (!__PX4_SBCAP(&_tail, candidate, next));
return true;
} else {
return false;
}
@@ -377,10 +390,12 @@ bool
RingBuffer::resize(unsigned new_size)
{
char *old_buffer;
char *new_buffer = new char [(new_size+1) * _item_size];
char *new_buffer = new char [(new_size + 1) * _item_size];
if (new_buffer == nullptr) {
return false;
}
old_buffer = _buf;
_buf = new_buffer;
_num_items = new_size;
src/drivers/device/ringbuffer.h
+4 -3
View File
@@ -46,7 +46,8 @@
namespace ringbuffer __EXPORT
{
class RingBuffer {
class RingBuffer
{
public:
RingBuffer(unsigned ring_size, size_t entry_size);
virtual ~RingBuffer();
@@ -171,8 +172,8 @@ private:
unsigned _next(unsigned index);
/* we don't want this class to be copied */
RingBuffer(const RingBuffer&);
RingBuffer operator=(const RingBuffer&);
RingBuffer(const RingBuffer &);
RingBuffer operator=(const RingBuffer &);
};
} // namespace ringbuffer
src/drivers/device/sim.cpp
+2 -2
View File
@@ -69,7 +69,7 @@ SIM::SIM(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
SIM::~SIM()
@@ -104,7 +104,7 @@ SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
if (recv_len > 0) {
PX4_DEBUG("SIM: receiving %d bytes", recv_len);
// TODO - write data to recv;
}
src/drivers/device/sim.h
+5 -5
View File
@@ -37,7 +37,7 @@
* Base class for devices on simulation bus.
*/
#pragma once
#pragma once
#include "vdev.h"
@@ -98,14 +98,14 @@ protected:
* otherwise.
*/
virtual int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
uint8_t *recv, unsigned recv_len);
private:
uint16_t _address;
const char * _devname;
const char *_devname;
SIM(const device::SIM&);
SIM operator=(const device::SIM&);
SIM(const device::SIM &);
SIM operator=(const device::SIM &);
};
} // namespace device
src/drivers/device/spi.cpp
+10 -6
View File
@@ -80,7 +80,7 @@ SPI::SPI(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = (uint8_t)device;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
SPI::~SPI()
@@ -94,8 +94,9 @@ SPI::init()
int ret = OK;
/* attach to the spi bus */
if (_dev == nullptr)
if (_dev == nullptr) {
_dev = up_spiinitialize(_bus);
}
if (_dev == nullptr) {
DEVICE_DEBUG("failed to init SPI");
@@ -141,34 +142,37 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
int result;
if ((send == nullptr) && (recv == nullptr))
if ((send == nullptr) && (recv == nullptr)) {
return -EINVAL;
}
LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
/* lock the bus as required */
switch (mode) {
default:
case LOCK_PREEMPTION:
{
case LOCK_PREEMPTION: {
irqstate_t state = irqsave();
result = _transfer(send, recv, len);
irqrestore(state);
}
break;
case LOCK_THREADS:
SPI_LOCK(_dev, true);
result = _transfer(send, recv, len);
SPI_LOCK(_dev, false);
break;
case LOCK_NONE:
result = _transfer(send, recv, len);
break;
}
return result;
}
void
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
src/drivers/device/spi.h
+4 -4
View File
@@ -109,12 +109,12 @@ protected:
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency);
void set_frequency(uint32_t frequency);
/**
* Locking modes supported by the driver.
@@ -134,8 +134,8 @@ private:
struct spi_dev_s *_dev;
/* this class does not allow copying */
SPI(const SPI&);
SPI operator=(const SPI&);
SPI(const SPI &);
SPI operator=(const SPI &);
protected:
int _bus;
src/drivers/device/vdev.cpp
+75 -36
View File
@@ -54,8 +54,9 @@ struct px4_dev_t {
char *name;
void *cdev;
px4_dev_t(const char *n, void *c) : cdev(c) {
name = strdup(n);
px4_dev_t(const char *n, void *c) : cdev(c)
{
name = strdup(n);
}
~px4_dev_t() { free(name); }
@@ -85,21 +86,26 @@ VDev::VDev(const char *name,
_open_count(0)
{
PX4_DEBUG("VDev::VDev");
for (unsigned i = 0; i < _max_pollwaiters; i++)
for (unsigned i = 0; i < _max_pollwaiters; i++) {
_pollset[i] = nullptr;
}
}
VDev::~VDev()
{
PX4_DEBUG("VDev::~VDev");
if (_registered)
if (_registered) {
unregister_driver(_devname);
}
}
int
VDev::register_class_devname(const char *class_devname)
{
PX4_DEBUG("VDev::register_class_devname %s", class_devname);
if (class_devname == nullptr) {
return -EINVAL;
}
@@ -111,13 +117,16 @@ VDev::register_class_devname(const char *class_devname)
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, (void *)this);
if (ret == OK) break;
if (ret == OK) { break; }
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
@@ -127,17 +136,19 @@ VDev::register_driver(const char *name, void *data)
PX4_DEBUG("VDev::register_driver %s", name);
int ret = -ENOSPC;
if (name == NULL || data == NULL)
if (name == NULL || data == NULL) {
return -EINVAL;
}
// Make sure the device does not already exist
// FIXME - convert this to a map for efficiency
for (int i=0;i<PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name,name) == 0)) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
return -EEXIST;
}
}
for (int i=0;i<PX4_MAX_DEV; ++i) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] == NULL) {
devmap[i] = new px4_dev_t(name, (void *)data);
PX4_DEBUG("Registered DEV %s", name);
@@ -145,9 +156,11 @@ VDev::register_driver(const char *name, void *data)
break;
}
}
if (ret != PX4_OK) {
PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
}
return ret;
}
@@ -157,10 +170,11 @@ VDev::unregister_driver(const char *name)
PX4_DEBUG("VDev::unregister_driver %s", name);
int ret = -EINVAL;
if (name == NULL)
if (name == NULL) {
return -EINVAL;
}
for (int i=0;i<PX4_MAX_DEV; ++i) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
delete devmap[i];
devmap[i] = NULL;
@@ -169,6 +183,7 @@ VDev::unregister_driver(const char *name)
break;
}
}
return ret;
}
@@ -178,14 +193,16 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
PX4_DEBUG("VDev::unregister_class_devname");
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
for (int i=0;i<PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name,name) == 0) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
delete devmap[i];
PX4_DEBUG("Unregistered class DEV %s", name);
devmap[i] = NULL;
return PX4_OK;
}
}
return -EINVAL;
}
@@ -197,15 +214,17 @@ VDev::init()
// base class init first
int ret = Device::init();
if (ret != PX4_OK)
if (ret != PX4_OK) {
goto out;
}
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, (void *)this);
if (ret != PX4_OK)
if (ret != PX4_OK) {
goto out;
}
_registered = true;
}
@@ -232,8 +251,9 @@ VDev::open(file_t *filep)
/* first-open callback may decline the open */
ret = open_first(filep);
if (ret != PX4_OK)
if (ret != PX4_OK) {
_open_count--;
}
}
unlock();
@@ -261,8 +281,9 @@ VDev::close(file_t *filep)
_open_count--;
/* callback cannot decline the close */
if (_open_count == 0)
if (_open_count == 0) {
ret = close_last(filep);
}
} else {
ret = -EBADF;
@@ -309,22 +330,26 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg)
switch (cmd) {
/* fetch a pointer to the driver's private data */
/* fetch a pointer to the driver's private data */
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
ret = PX4_OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
ret = PX4_OK;
break;
case DEVIOCGPUBBLOCK:
ret = _pub_blocked;
break;
case DEVIOCGDEVICEID:
ret = (int)_device_id.devid;
case DEVIOCGDEVICEID:
ret = (int)_device_id.devid;
PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret);
break;
default:
break;
}
@@ -365,8 +390,10 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
fds->revents |= fds->events & poll_state(filep);
/* yes? post the notification */
if (fds->revents != 0)
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
} else {
PX4_WARN("Store Poll Waiter error.");
}
@@ -392,8 +419,9 @@ VDev::poll_notify(pollevent_t events)
lock();
for (unsigned i = 0; i < _max_pollwaiters; i++)
if (nullptr != _pollset[i])
if (nullptr != _pollset[i]) {
poll_notify_one(_pollset[i], events);
}
unlock();
}
@@ -408,7 +436,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
/* update the reported event set */
fds->revents |= fds->events & events;
PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value);
PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value);
/* if the state is now interesting, wake the waiter if it's still asleep */
/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
@@ -432,6 +460,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
* Look for a free slot.
*/
PX4_DEBUG("VDev::store_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (nullptr == _pollset[i]) {
@@ -449,6 +478,7 @@ int
VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
{
PX4_DEBUG("VDev::remove_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (fds == _pollset[i]) {
@@ -465,8 +495,9 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
VDev *VDev::getDev(const char *path)
{
PX4_DEBUG("VDev::getDev");
int i=0;
for (; i<PX4_MAX_DEV; ++i) {
int i = 0;
for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path);
//}
@@ -474,14 +505,16 @@ VDev *VDev::getDev(const char *path)
return (VDev *)(devmap[i]->cdev);
}
}
return NULL;
}
void VDev::showDevices()
{
int i=0;
int i = 0;
PX4_INFO("Devices:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
@@ -490,9 +523,10 @@ void VDev::showDevices()
void VDev::showTopics()
{
int i=0;
int i = 0;
PX4_INFO("Devices:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
@@ -501,11 +535,12 @@ void VDev::showTopics()
void VDev::showFiles()
{
int i=0;
int i = 0;
PX4_INFO("Files:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
@@ -513,17 +548,21 @@ void VDev::showFiles()
const char *VDev::topicList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0)
for (; *next < PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0) {
return devmap[(*next)++]->name;
}
return NULL;
}
const char *VDev::devList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0)
for (; *next < PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0) {
return devmap[(*next)++]->name;
}
return NULL;
}
src/drivers/device/vdev.h
+34 -29
View File
@@ -37,7 +37,7 @@
* Definitions for the generic base classes in the virtual device framework.
*/
#pragma once
#pragma once
/*
* Includes here should only cover the needs of the framework definitions.
@@ -118,17 +118,17 @@ public:
* @param data The buffer from which values should be read.
* @param count The number of items to write.
* @return The number of items written on success, negative errno otherwise.
*/
*/
virtual int dev_write(unsigned address, void *data, unsigned count);
/**
* Perform a device-specific operation.
*
* @param operation The operation to perform.
* @param arg An argument to the operation.
* @return Negative errno on error, OK or positive value on success.
*/
virtual int dev_ioctl(unsigned operation, unsigned &arg);
/**
* Perform a device-specific operation.
*
* @param operation The operation to perform.
* @param arg An argument to the operation.
* @return Negative errno on error, OK or positive value on success.
*/
virtual int dev_ioctl(unsigned operation, unsigned &arg);
/*
device bus types for DEVID
@@ -148,13 +148,13 @@ public:
parameter protocol without loss of information.
*/
struct DeviceStructure {
enum DeviceBusType bus_type:3;
uint8_t bus:5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
enum DeviceBusType bus_type : 3;
uint8_t bus: 5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
union DeviceId {
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
@@ -179,7 +179,8 @@ protected:
*
* Note that we must loop as the wait may be interrupted by a signal.
*/
void lock() {
void lock()
{
DEVICE_DEBUG("lock");
do {} while (px4_sem_wait(&_lock) != 0);
}
@@ -187,7 +188,8 @@ protected:
/**
* Release the driver lock.
*/
void unlock() {
void unlock()
{
DEVICE_DEBUG("unlock");
px4_sem_post(&_lock);
}
@@ -330,7 +332,7 @@ public:
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
const char *get_devname() { return _devname; }
protected:
@@ -395,7 +397,7 @@ protected:
*/
virtual int close_last(file_t *filep);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -404,7 +406,7 @@ protected:
*/
virtual int register_class_devname(const char *class_devname);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -442,7 +444,7 @@ private:
int remove_poll_waiter(px4_pollfd_struct_t *fds);
/* do not allow copying this class */
VDev(const VDev&);
VDev(const VDev &);
//VDev operator=(const VDev&);
};
@@ -464,7 +466,7 @@ public:
PIO(const char *name,
const char *devname,
unsigned long base
);
);
virtual ~PIO();
virtual int init();
@@ -476,7 +478,8 @@ protected:
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
uint32_t reg(uint32_t offset)
{
return *(volatile uint32_t *)(_base + offset);
}
@@ -486,7 +489,8 @@ protected:
* @param offset Register offset in bytes from the base address.
* @param value Value to write.
*/
void reg(uint32_t offset, uint32_t value) {
void reg(uint32_t offset, uint32_t value)
{
*(volatile uint32_t *)(_base + offset) = value;
}
@@ -500,7 +504,8 @@ protected:
* @param clearbits Bits to clear in the register
* @param setbits Bits to set in the register
*/
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) {
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits)
{
uint32_t val = reg(offset);
val &= ~clearbits;
val |= setbits;
@@ -516,8 +521,8 @@ private:
// class instance for primary driver of each class
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
CLASS_DEVICE_PRIMARY = 0,
CLASS_DEVICE_SECONDARY = 1,
CLASS_DEVICE_TERTIARY = 2
};
src/drivers/device/vdev_posix.cpp
+236 -225
View File
@@ -54,254 +54,265 @@ using namespace device;
extern "C" {
static void timer_cb(void *data)
{
px4_sem_t *p_sem = (px4_sem_t *)data;
px4_sem_post(p_sem);
PX4_DEBUG("timer_handler: Timer expired");
}
static void timer_cb(void *data)
{
px4_sem_t *p_sem = (px4_sem_t *)data;
px4_sem_post(p_sem);
PX4_DEBUG("timer_handler: Timer expired");
}
#define PX4_MAX_FD 200
static device::file_t *filemap[PX4_MAX_FD] = {};
static device::file_t *filemap[PX4_MAX_FD] = {};
int px4_errno;
int px4_errno;
inline bool valid_fd(int fd)
{
return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
}
int px4_open(const char *path, int flags, ...)
{
PX4_DEBUG("px4_open");
VDev *dev = VDev::getDev(path);
int ret = 0;
int i;
mode_t mode;
if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 &&
strncmp(path, "/obj/", 5) != 0 &&
strncmp(path, "/dev/", 5) != 0)
inline bool valid_fd(int fd)
{
va_list p;
va_start(p, flags);
mode = va_arg(p, mode_t);
va_end(p);
return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
}
// Create the file
PX4_DEBUG("Creating virtual file %s", path);
dev = VFile::createFile(path, mode);
}
if (dev) {
for (i=0; i<PX4_MAX_FD; ++i) {
if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags,dev,i);
break;
}
}
if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]);
}
else {
PX4_WARN("exceeded maximum number of file descriptors!");
ret = -ENOENT;
}
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
return -1;
}
PX4_DEBUG("px4_open fd = %d", filemap[i]->fd);
return filemap[i]->fd;
}
int px4_close(int fd)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd);
ret = dev->close(filemap[fd]);
filemap[fd] = NULL;
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_read(int fd, void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
int px4_ioctl(int fd, int cmd, unsigned long arg)
{
PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
ret = dev->ioctl(filemap[fd], cmd, arg);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
}
return ret;
}
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
px4_sem_t sem;
int count = 0;
int ret = -1;
unsigned int i;
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
px4_sem_init(&sem, 0, 0);
// For each fd
for (i=0; i<nfds; ++i)
int px4_open(const char *path, int flags, ...)
{
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
PX4_DEBUG("px4_open");
VDev *dev = VDev::getDev(path);
int ret = 0;
int i;
mode_t mode;
// If fd is valid
if (valid_fd(fds[i].fd))
{
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 &&
strncmp(path, "/obj/", 5) != 0 &&
strncmp(path, "/dev/", 5) != 0) {
va_list p;
va_start(p, flags);
mode = va_arg(p, mode_t);
va_end(p);
if (ret < 0)
break;
}
}
if (ret >= 0)
{
if (timeout > 0)
{
// Use a work queue task
work_s _hpwork;
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout);
px4_sem_wait(&sem);
// Make sure timer thread is killed before sem goes
// out of scope
hrt_work_cancel(&_hpwork);
}
else if (timeout < 0)
{
px4_sem_wait(&sem);
// Create the file
PX4_DEBUG("Creating virtual file %s", path);
dev = VFile::createFile(path, mode);
}
// For each fd
for (i=0; i<nfds; ++i)
{
// If fd is valid
if (valid_fd(fds[i].fd))
{
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
if (ret < 0)
if (dev) {
for (i = 0; i < PX4_MAX_FD; ++i) {
if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags, dev, i);
break;
}
}
if (fds[i].revents)
count += 1;
if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]);
} else {
PX4_WARN("exceeded maximum number of file descriptors!");
ret = -ENOENT;
}
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
return -1;
}
PX4_DEBUG("px4_open fd = %d", filemap[i]->fd);
return filemap[i]->fd;
}
int px4_close(int fd)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd);
ret = dev->close(filemap[fd]);
filemap[fd] = NULL;
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_read(int fd, void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
int px4_ioctl(int fd, int cmd, unsigned long arg)
{
PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
ret = dev->ioctl(filemap[fd], cmd, arg);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
}
return ret;
}
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
px4_sem_t sem;
int count = 0;
int ret = -1;
unsigned int i;
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
px4_sem_init(&sem, 0, 0);
// For each fd
for (i = 0; i < nfds; ++i) {
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
if (ret < 0) {
break;
}
}
}
if (ret >= 0) {
if (timeout > 0) {
// Use a work queue task
work_s _hpwork;
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout);
px4_sem_wait(&sem);
// Make sure timer thread is killed before sem goes
// out of scope
hrt_work_cancel(&_hpwork);
} else if (timeout < 0) {
px4_sem_wait(&sem);
}
// For each fd
for (i = 0; i < nfds; ++i) {
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
if (ret < 0) {
break;
}
if (fds[i].revents) {
count += 1;
}
}
}
}
px4_sem_destroy(&sem);
return count;
}
px4_sem_destroy(&sem);
return count;
}
int px4_fsync(int fd)
{
return 0;
}
int px4_access(const char *pathname, int mode)
{
if (mode != F_OK) {
errno = EINVAL;
return -1;
int px4_fsync(int fd)
{
return 0;
}
VDev *dev = VDev::getDev(pathname);
return (dev != nullptr) ? 0 : -1;
}
void px4_show_devices()
{
VDev::showDevices();
}
int px4_access(const char *pathname, int mode)
{
if (mode != F_OK) {
errno = EINVAL;
return -1;
}
void px4_show_topics()
{
VDev::showTopics();
}
VDev *dev = VDev::getDev(pathname);
return (dev != nullptr) ? 0 : -1;
}
void px4_show_files()
{
VDev::showFiles();
}
void px4_show_devices()
{
VDev::showDevices();
}
const char * px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
}
void px4_show_topics()
{
VDev::showTopics();
}
const char * px4_get_topic_names(unsigned int *handle)
{
return VDev::topicList(handle);
}
void px4_show_files()
{
VDev::showFiles();
}
const char *px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
}
const char *px4_get_topic_names(unsigned int *handle)
{
return VDev::topicList(handle);
}
}
src/drivers/device/vfile.cpp
+1 -1
View File
@@ -48,7 +48,7 @@ VFile::VFile(const char *fname, mode_t mode) :
{
}
VFile * VFile::createFile(const char *fname, mode_t mode)
VFile *VFile::createFile(const char *fname, mode_t mode)
{
VFile *me = new VFile(fname, mode);
me->register_driver(fname, me);
src/drivers/device/vfile.h
+2 -1
View File
@@ -44,7 +44,8 @@
#include <unistd.h>
#include <stdio.h>
class VFile : public device::VDev {
class VFile : public device::VDev
{
public:
static VFile *createFile(const char *fname, mode_t mode);
src/drivers/drv_device.h
+1 -1
View File
@@ -68,7 +68,7 @@
#ifdef __PX4_POSIX
#ifndef SIOCDEVPRIVATE
#define SIOCDEVPRIVATE 1
#define SIOCDEVPRIVATE 1
#endif
#define DIOC_GETPRIV SIOCDEVPRIVATE
src/drivers/drv_pwm_output.h
+1 -1
View File
@@ -65,7 +65,7 @@ __BEGIN_DECLS
#define pwm_output_values output_pwm_s
#ifndef PWM_OUTPUT_MAX_CHANNELS
#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS
#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS
#endif
/**
src/drivers/ets_airspeed/ets_airspeed.cpp
+1 -1
View File
@@ -162,7 +162,7 @@ ETSAirspeed::collect()
// caller could end up using this value as part of an
// average
perf_count(_comms_errors);
DEVICE_LOG("zero value from sensor");
DEVICE_LOG("zero value from sensor");
return -1;
}
src/drivers/frsky_telemetry/frsky_telemetry.c
+19 -15
View File
@@ -83,6 +83,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
close(uart);
@@ -120,9 +121,9 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
static void usage()
{
fprintf(stderr,
"usage: frsky_telemetry start [-d <devicename>]\n"
" frsky_telemetry stop\n"
" frsky_telemetry status\n");
"usage: frsky_telemetry start [-d <devicename>]\n"
" frsky_telemetry stop\n"
" frsky_telemetry status\n");
exit(1);
}
@@ -139,6 +140,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "d:")) != EOF) {
switch (ch) {
case 'd':
@@ -155,8 +157,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
if (uart < 0)
if (uart < 0) {
err(1, "could not open %s", device_name);
}
/* Subscribe to topics */
frsky_init();
@@ -165,6 +168,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* Main thread loop */
unsigned int iteration = 0;
while (!thread_should_exit) {
/* Sleep 200 ms */
@@ -174,14 +178,12 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
frsky_send_frame1(uart);
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0)
{
if (iteration % 5 == 0) {
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0)
{
if (iteration % 25 == 0) {
frsky_send_frame3(uart);
iteration = 0;
@@ -212,16 +214,17 @@ int frsky_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running)
if (thread_running) {
errx(0, "frsky_telemetry already running");
}
thread_should_exit = false;
frsky_task = px4_task_spawn_cmd("frsky_telemetry",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
frsky_telemetry_thread_main,
(char * const *)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
frsky_telemetry_thread_main,
(char *const *)argv);
while (!thread_running) {
usleep(200);
@@ -233,8 +236,9 @@ int frsky_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running)
if (!thread_running) {
errx(0, "frsky_telemetry already stopped");
}
thread_should_exit = true;
src/drivers/gimbal/gimbal.cpp
+6 -5
View File
@@ -331,7 +331,7 @@ Gimbal::cycle()
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
@@ -358,20 +358,21 @@ Gimbal::cycle()
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
(double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
src/drivers/gps/ashtech.cpp
+26 -14
View File
@@ -37,7 +37,7 @@ ASHTECH::~ASHTECH()
int ASHTECH::handle_message(int len)
{
char * endp;
char *endp;
if (len < 7) { return 0; }
@@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len)
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
*/
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0,
local_time_off_min __attribute__((unused)) = 0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
@@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len)
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = usecs * 1000;
if (clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += usecs;
} else {
_gps_position->time_utc_usec = 0;
}
@@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len)
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9,
tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
@@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len)
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
*/
lat = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
@@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len)
if (bufptr && *(++bufptr) != ',') {
lon = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
@@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len)
if (bufptr && *(++bufptr) != ',') {
alt = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
@@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len)
_gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */
_gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */
_gps_position->vel_d_m_s = static_cast<float>(-vertic_vel); /** GPS ground speed in m/s */
_gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->cog_rad =
track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1f;
_gps_position->timestamp_velocity = hrt_absolute_time();
@@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len)
9 The checksum data, always begins with *
*/
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0,
deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
@@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len)
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
_gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
+ static_cast<float>(lon_err) * static_cast<float>(lon_err));
+ static_cast<float>(lon_err) * static_cast<float>(lon_err));
_gps_position->epv = static_cast<float>(alt_err);
_gps_position->s_variance_m_s = 0;
@@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b)
int iRet = 0;
switch (_decode_state) {
/* First, look for sync1 */
/* First, look for sync1 */
case NME_DECODE_UNINIT:
if (b == '$') {
_decode_state = NME_DECODE_GOT_SYNC1;
@@ -636,13 +648,13 @@ void ASHTECH::decode_init(void)
*/
const char comm[] = "$PASHS,POP,20\r\n"\
"$PASHS,NME,ZDA,B,ON,3\r\n"\
"$PASHS,NME,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\
"$PASHS,NME,POS,B,ON,0.05\r\n"\
"$PASHS,NME,GSV,B,ON,3\r\n"\
"$PASHS,SPD,A,8\r\n"\
"$PASHS,SPD,B,9\r\n";
"$PASHS,NME,ZDA,B,ON,3\r\n"\
"$PASHS,NME,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\
"$PASHS,NME,POS,B,ON,0.05\r\n"\
"$PASHS,NME,GSV,B,ON,3\r\n"\
"$PASHS,SPD,A,8\r\n"\
"$PASHS,SPD,B,9\r\n";
int ASHTECH::configure(unsigned &baudrate)
{
src/drivers/gps/gps.cpp
+46 -29
View File
@@ -211,8 +211,9 @@ GPS::~GPS()
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
if (_task != -1) {
task_delete(_task);
}
g_dev = nullptr;
@@ -224,12 +225,13 @@ GPS::init()
int ret = ERROR;
/* do regular cdev init */
if (CDev::init() != OK)
if (CDev::init() != OK) {
goto out;
}
/* start the GPS driver worker task */
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr);
SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
@@ -305,9 +307,10 @@ GPS::task_main()
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
_report_gps_pos.vel_ned_valid = true;
/* no time and satellite information simulated */
@@ -392,14 +395,16 @@ GPS::task_main()
}
int helper_ret;
while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
if (helper_ret & 1) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
}
if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
@@ -508,7 +513,7 @@ void
GPS::print_info()
{
//GPS Mode
if(_fake_gps) {
if (_fake_gps) {
warnx("protocol: SIMULATED");
}
@@ -522,27 +527,27 @@ GPS::print_info()
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH");
break;
case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH");
break;
default:
break;
}
}
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
warnx("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
@@ -575,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init())
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(GPS0_DEVICE_PATH, O_RDONLY);
@@ -639,11 +647,13 @@ reset()
{
int fd = open(GPS0_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "reset failed");
}
exit(0);
}
@@ -654,8 +664,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "not running");
}
g_dev->print_info();
@@ -690,39 +701,45 @@ gps_main(int argc, char *argv[])
/* Detect fake gps option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f"))
if (!strcmp(argv[i], "-f")) {
fake_gps = true;
}
}
/* Detect sat info option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s"))
if (!strcmp(argv[i], "-s")) {
enable_sat_info = true;
}
}
gps::start(device_name, fake_gps, enable_sat_info);
}
if (!strcmp(argv[1], "stop"))
if (!strcmp(argv[1], "stop")) {
gps::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
gps::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
gps::reset();
}
/*
* Print driver status.
*/
if (!strcmp(argv[1], "status"))
if (!strcmp(argv[1], "status")) {
gps::info();
}
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
src/drivers/gps/mtk.cpp
+6 -2
View File
@@ -66,8 +66,9 @@ int
MTK::configure(unsigned &baudrate)
{
/* set baudrate first */
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) {
return -1;
}
baudrate = MTK_BAUDRATE;
@@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
// Add to checksum
if (_rx_count < 33)
if (_rx_count < 33) {
add_byte_to_checksum(b);
}
// Fill packet buffer
((uint8_t *)(&packet))[_rx_count] = b;
@@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet)
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
if (clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
} else {
_gps_position->time_utc_usec = 0;
}
src/drivers/gps/ubx.cpp
+159 -81
View File
@@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate)
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
@@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate)
/* try to set rate for NAV-PVT */
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
configure_message_rate(UBX_MSG_NAV_PVT, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
_use_nav_pvt = false;
} else {
_use_nav_pvt = true;
}
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_SOL, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
}
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_MON_HW, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
@@ -269,9 +279,11 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
if (_ack_state == UBX_ACK_GOT_ACK) {
ret = 0; // ACK received ok
} else if (report) {
if (_ack_state == UBX_ACK_GOT_NAK) {
UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
} else {
UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
}
@@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b)
UBX_TRACE_PARSER("\nA");
_decode_state = UBX_DECODE_SYNC2;
}
break;
/* Expecting Sync2 */
@@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b)
} else { // Sync1 not followed by Sync2: reset parser
decode_init();
}
break;
/* Expecting Class */
@@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b)
UBX_TRACE_PARSER("F");
add_byte_to_checksum(b);
_rx_payload_length |= b << 8; // calculate payload size
if (payload_rx_init() != 0) { // start payload reception
// payload will not be handled, discard message
decode_init();
} else {
_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
}
break;
/* Expecting payload */
case UBX_DECODE_PAYLOAD:
UBX_TRACE_PARSER(".");
add_byte_to_checksum(b);
switch (_rx_msg) {
case UBX_MSG_NAV_SVINFO:
ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte
break;
case UBX_MSG_MON_VER:
ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte
break;
default:
ret = payload_rx_add(b); // add a payload byte
break;
}
if (ret < 0) {
// payload not handled, discard message
decode_init();
} else if (ret > 0) {
// payload complete, expecting checksum
_decode_state = UBX_DECODE_CHKSUM1;
} else {
// expecting more payload, stay in state UBX_DECODE_PAYLOAD
}
ret = 0;
break;
@@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b)
if (_rx_ck_a != b) {
UBX_WARN("ubx checksum err");
decode_init();
} else {
_decode_state = UBX_DECODE_CHKSUM2;
}
break;
/* Expecting second checksum byte */
case UBX_DECODE_CHKSUM2:
if (_rx_ck_b != b) {
UBX_WARN("ubx checksum err");
} else {
ret = payload_rx_done(); // finish payload processing
}
decode_init();
break;
@@ -475,83 +503,116 @@ UBX::payload_rx_init()
switch (_rx_msg) {
case UBX_MSG_NAV_PVT:
if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */
if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (!_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (!_use_nav_pvt) {
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
}
break;
case UBX_MSG_NAV_POSLLH:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (_use_nav_pvt) {
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
}
break;
case UBX_MSG_NAV_SOL:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (_use_nav_pvt) {
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
}
break;
case UBX_MSG_NAV_TIMEUTC:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (_use_nav_pvt) {
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
}
break;
case UBX_MSG_NAV_SVINFO:
if (_satellite_info == nullptr)
_rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else
memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
if (_satellite_info == nullptr) {
_rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else {
memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
}
break;
case UBX_MSG_NAV_VELNED:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (_use_nav_pvt) {
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
}
break;
case UBX_MSG_MON_VER:
break; // unconditionally handle this message
case UBX_MSG_MON_HW:
if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */
if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
}
break;
case UBX_MSG_ACK_ACK:
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
} else if (_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
}
break;
case UBX_MSG_ACK_NAK:
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
} else if (_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
}
break;
default:
@@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b)
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
// Fill Part 1 buffer
_buf.raw[_rx_payload_index] = b;
} else {
if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
// Part 1 complete: decode Part 1 buffer
_satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES);
UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length,
(unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
}
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(
ubx_payload_rx_nav_svinfo_part2_t)) {
// Still room in _satellite_info: fill Part 2 buffer
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(
ubx_payload_rx_nav_svinfo_part2_t);
_buf.raw[buf_index] = b;
if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
// Part 2 complete: decode Part 2 buffer
unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(
ubx_payload_rx_nav_svinfo_part2_t);
_satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
_satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno);
_satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev);
_satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f);
_satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid);
UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n",
(unsigned)sat_index + 1,
(unsigned)_satellite_info->used[sat_index],
(unsigned)_satellite_info->snr[sat_index],
(unsigned)_satellite_info->elevation[sat_index],
(unsigned)_satellite_info->azimuth[sat_index],
(unsigned)_satellite_info->svid[sat_index]
);
(unsigned)sat_index + 1,
(unsigned)_satellite_info->used[sat_index],
(unsigned)_satellite_info->snr[sat_index],
(unsigned)_satellite_info->elevation[sat_index],
(unsigned)_satellite_info->azimuth[sat_index],
(unsigned)_satellite_info->svid[sat_index]
);
}
}
}
@@ -672,6 +740,7 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
// Fill Part 1 buffer
_buf.raw[_rx_payload_index] = b;
} else {
if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
@@ -681,9 +750,12 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
}
// fill Part 2 buffer
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(
ubx_payload_rx_mon_ver_part2_t);
_buf.raw[buf_index] = b;
if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
// Part 2 complete: decode Part 2 buffer
UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
@@ -717,13 +789,11 @@ UBX::payload_rx_done(void)
UBX_TRACE_RXMSG("Rx NAV-PVT\n");
//Check if position fix flag is good
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1)
{
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) {
_gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
_gps_position->vel_ned_valid = true;
}
else
{
} else {
_gps_position->fix_type = 0;
_gps_position->vel_ned_valid = false;
}
@@ -748,10 +818,9 @@ UBX::payload_rx_done(void)
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
//Check if time and date fix flags are good
if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED))
{
if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) {
/* convert to unix timestamp */
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
@@ -770,12 +839,14 @@ UBX::payload_rx_done(void)
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
if (clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
} else {
_gps_position->time_utc_usec = 0;
}
@@ -827,8 +898,7 @@ UBX::payload_rx_done(void)
case UBX_MSG_NAV_TIMEUTC:
UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC)
{
if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) {
// convert to unix timestamp
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
@@ -849,12 +919,14 @@ UBX::payload_rx_done(void)
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
if (clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
} else {
_gps_position->time_utc_usec = 0;
}
@@ -922,6 +994,7 @@ UBX::payload_rx_done(void)
ret = 0; // don't handle message
break;
}
break;
case UBX_MSG_ACK_ACK:
@@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len
header.length = length;
// Calculate checksum
calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
if (payload != nullptr)
calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
if (payload != nullptr) {
calc_checksum(payload, length, &checksum);
}
// Send message
write(_fd, (const void *)&header, sizeof(header));
if (payload != nullptr)
if (payload != nullptr) {
write(_fd, (const void *)payload, length);
}
write(_fd, (const void *)&checksum, sizeof(checksum));
}
uint32_t
UBX::fnv1_32_str(uint8_t *str, uint32_t hval)
{
uint8_t *s = str;
uint8_t *s = str;
/*
* FNV-1 hash each octet in the buffer
*/
while (*s) {
/*
* FNV-1 hash each octet in the buffer
*/
while (*s) {
/* multiply by the 32 bit FNV magic prime mod 2^32 */
/* multiply by the 32 bit FNV magic prime mod 2^32 */
#if defined(NO_FNV_GCC_OPTIMIZATION)
hval *= FNV1_32_PRIME;
hval *= FNV1_32_PRIME;
#else
hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24);
hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24);
#endif
/* xor the bottom with the current octet */
hval ^= (uint32_t)*s++;
}
/* xor the bottom with the current octet */
hval ^= (uint32_t) * s++;
}
/* return our new hash value */
return hval;
/* return our new hash value */
return hval;
}
src/drivers/hmc5883/hmc5883.cpp
+189 -107
View File
@@ -321,25 +321,25 @@ private:
*
* @return 0 if calibration is ok, 1 else
*/
int check_calibration();
int check_calibration();
/**
* Check the current scale calibration
*
* @return 0 if scale calibration is ok, 1 else
*/
int check_scale();
/**
* Check the current scale calibration
*
* @return 0 if scale calibration is ok, 1 else
*/
int check_scale();
/**
* Check the current offset calibration
*
* @return 0 if offset calibration is ok, 1 else
*/
int check_offset();
/**
* Check the current offset calibration
*
* @return 0 if offset calibration is ok, 1 else
*/
int check_offset();
/* this class has pointer data members, do not allow copying it */
HMC5883(const HMC5883&);
HMC5883 operator=(const HMC5883&);
HMC5883(const HMC5883 &);
HMC5883 operator=(const HMC5883 &);
};
/*
@@ -397,11 +397,13 @@ HMC5883::~HMC5883()
/* make sure we are truly inactive */
stop();
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1)
if (_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
@@ -417,6 +419,7 @@ HMC5883::init()
int ret = ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
@@ -424,8 +427,10 @@ HMC5883::init()
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
}
/* reset the device configuration */
reset();
@@ -489,14 +494,16 @@ int HMC5883::set_range(unsigned range)
*/
ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
uint8_t range_bits_in = 0;
ret = read_reg(ADDR_CONF_B, range_bits_in);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
return !(range_bits_in == (_range_bits << 5));
}
@@ -512,15 +519,19 @@ void HMC5883::check_range(void)
uint8_t range_bits_in = 0;
ret = read_reg(ADDR_CONF_B, range_bits_in);
if (OK != ret) {
perf_count(_comms_errors);
return;
}
if (range_bits_in != (_range_bits<<5)) {
if (range_bits_in != (_range_bits << 5)) {
perf_count(_range_errors);
ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
}
}
@@ -535,15 +546,19 @@ void HMC5883::check_conf(void)
uint8_t conf_reg_in = 0;
ret = read_reg(ADDR_CONF_A, conf_reg_in);
if (OK != ret) {
perf_count(_comms_errors);
return;
}
if (conf_reg_in != _conf_reg) {
perf_count(_conf_errors);
ret = write_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
}
}
@@ -555,8 +570,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
@@ -611,77 +627,84 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
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);
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(HMC5883_CONVERSION_INTERVAL);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
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);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
return OK;
}
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000/TICK2USEC(_measure_ticks);
return 1000000 / TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
@@ -699,7 +722,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
return 1000000 / TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
@@ -815,8 +838,9 @@ HMC5883::cycle()
}
/* measurement phase */
if (OK != measure())
if (OK != measure()) {
DEVICE_DEBUG("measure error");
}
/* next phase is collection */
_collect_phase = true;
@@ -839,8 +863,9 @@ HMC5883::measure()
*/
ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
return ret;
}
@@ -872,7 +897,7 @@ HMC5883::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
@@ -908,6 +933,7 @@ HMC5883::collect()
/* get measurements from the device */
new_report.temperature = 0;
if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) {
/*
if temperature compensation is enabled read the
@@ -923,13 +949,16 @@ HMC5883::collect()
ret = _interface->read(ADDR_TEMP_OUT_MSB,
raw_temperature, sizeof(raw_temperature));
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
raw_temperature[1];
new_report.temperature = 25 + (temp16 / (16*8.0f));
raw_temperature[1];
new_report.temperature = 25 + (temp16 / (16 * 8.0f));
_temperature_error_count = 0;
} else {
_temperature_error_count++;
if (_temperature_error_count == 10) {
/*
it probably really is an old HMC5883,
@@ -940,6 +969,7 @@ HMC5883::collect()
set_temperature_compensation(0);
}
}
} else {
new_report.temperature = _last_report.temperature;
}
@@ -961,17 +991,18 @@ HMC5883::collect()
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
}
/* the standard external mag by 3DR has x pointing to the
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
xraw_f = -report.y;
xraw_f = -report.y;
yraw_f = report.x;
zraw_f = report.z;
@@ -989,12 +1020,14 @@ HMC5883::collect()
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr)
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
@@ -1016,9 +1049,11 @@ HMC5883::collect()
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_range();
}
if (check_counter == 128) {
check_conf();
}
@@ -1075,7 +1110,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
}
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
@@ -1146,9 +1181,11 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
fabsf(expected_cal[2] / report.z)
};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
@@ -1211,10 +1248,11 @@ int HMC5883::check_scale()
bool scale_valid;
if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
/* scale is one */
scale_valid = false;
} else {
scale_valid = true;
}
@@ -1228,10 +1266,11 @@ int HMC5883::check_offset()
bool offset_valid;
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
/* offset is zero */
offset_valid = false;
} else {
offset_valid = true;
}
@@ -1247,7 +1286,7 @@ int HMC5883::check_calibration()
if (_calibrated != (offset_valid && scale_valid)) {
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
}
@@ -1261,10 +1300,12 @@ int HMC5883::set_excitement(unsigned enable)
/* arm the excitement strap */
ret = read_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
_conf_reg &= ~0x03; // reset previous excitement mode
if (((int)enable) < 0) {
_conf_reg |= 0x01;
@@ -1273,12 +1314,13 @@ int HMC5883::set_excitement(unsigned enable)
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
ret = write_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors);
}
uint8_t conf_reg_ret = 0;
read_reg(ADDR_CONF_A, conf_reg_ret);
@@ -1317,11 +1359,12 @@ int HMC5883::set_temperature_compensation(unsigned enable)
if (OK != ret) {
perf_count(_comms_errors);
return -EIO;
}
return -EIO;
}
if (enable != 0) {
_conf_reg |= HMC5983_TEMP_SENSOR_ENABLE;
} else {
_conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE;
}
@@ -1334,6 +1377,7 @@ int HMC5883::set_temperature_compensation(unsigned enable)
}
uint8_t conf_reg_ret = 0;
if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) {
perf_count(_comms_errors);
return -EIO;
@@ -1383,7 +1427,7 @@ HMC5883::print_info()
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)(1.0f/_range_scale), (double)_range_ga);
(double)(1.0f / _range_scale), (double)_range_ga);
printf("temperature %.2f\n", (double)_last_report.temperature);
_reports->print_info("report queue");
}
@@ -1436,16 +1480,20 @@ void usage();
bool
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
{
if (bus.dev != nullptr)
errx(1,"bus option already started");
if (bus.dev != nullptr) {
errx(1, "bus option already started");
}
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new HMC5883(interface, bus.devpath, rotation);
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
@@ -1453,14 +1501,16 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
}
int fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
return false;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1,"Failed to setup poll rate");
errx(1, "Failed to setup poll rate");
}
close(fd);
return true;
@@ -1483,10 +1533,12 @@ start(enum HMC5883_BUS busid, enum Rotation rotation)
// this device is already started
continue;
}
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
}
@@ -1506,6 +1558,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
return bus_options[i];
}
}
errx(1, "bus %u not started", (unsigned)busid);
}
@@ -1526,31 +1579,37 @@ test(enum HMC5883_BUS busid)
int fd = open(path, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'hmc5883 start')", path);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
/* check if mag is onboard or external */
if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
errx(1, "failed to get if mag is onboard or external");
}
warnx("device active: %s", ret ? "external" : "onboard");
/* set the queue depth to 5 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
errx(1, "failed to set queue depth");
}
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
@@ -1561,14 +1620,16 @@ test(enum HMC5883_BUS busid)
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
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))
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
@@ -1628,8 +1689,9 @@ int calibrate(enum HMC5883_BUS busid)
int fd = open(path, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
}
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@@ -1651,14 +1713,17 @@ reset(enum HMC5883_BUS busid)
int fd = open(path, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
@@ -1675,11 +1740,13 @@ temp_enable(enum HMC5883_BUS busid, bool enable)
int fd = open(path, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0)
if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) {
err(1, "set temperature compensation failed");
}
close(fd);
return 0;
@@ -1719,7 +1786,7 @@ hmc5883_main(int argc, char *argv[])
int ch;
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
bool calibrate = false;
bool temp_compensation = false;
while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) {
@@ -1728,22 +1795,28 @@ hmc5883_main(int argc, char *argv[])
rotation = (enum Rotation)atoi(optarg);
break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
case 'I':
busid = HMC5883_BUS_I2C_INTERNAL;
break;
#endif
case 'X':
busid = HMC5883_BUS_I2C_EXTERNAL;
break;
case 'S':
busid = HMC5883_BUS_SPI;
break;
case 'C':
calibrate = true;
break;
case 'T':
temp_compensation = true;
break;
default:
hmc5883::usage();
exit(0);
@@ -1757,42 +1830,51 @@ hmc5883_main(int argc, char *argv[])
*/
if (!strcmp(verb, "start")) {
hmc5883::start(busid, rotation);
if (calibrate && hmc5883::calibrate(busid) != 0) {
errx(1, "calibration failed");
}
if (temp_compensation) {
// we consider failing to setup temperature
// compensation as non-fatal
hmc5883::temp_enable(busid, true);
}
exit(0);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
if (!strcmp(verb, "test")) {
hmc5883::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
if (!strcmp(verb, "reset")) {
hmc5883::reset(busid);
}
/*
* enable/disable temperature compensation
*/
if (!strcmp(verb, "tempoff"))
if (!strcmp(verb, "tempoff")) {
hmc5883::temp_enable(busid, false);
if (!strcmp(verb, "tempon"))
}
if (!strcmp(verb, "tempon")) {
hmc5883::temp_enable(busid, true);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
hmc5883::info(busid);
}
/*
* Autocalibrate the scaling
src/drivers/hmc5883/hmc5883_i2c.cpp
+9 -7
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file HMC5883_I2C.cpp
*
* I2C interface for HMC5883 / HMC 5983
*/
/**
* @file HMC5883_I2C.cpp
*
* I2C interface for HMC5883 / HMC 5983
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -72,7 +72,7 @@ public:
virtual ~HMC5883_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
@@ -118,11 +118,13 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (_bus == PX4_I2C_BUS_EXPANSION) {
return 1;
} else {
return 0;
}
#else
return 1;
return 1;
#endif
case DEVIOCGDEVICEID:
src/drivers/hmc5883/hmc5883_spi.cpp
+11 -11
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file HMC5883_SPI.cpp
*
* SPI interface for HMC5983
*/
/**
* @file HMC5883_SPI.cpp
*
* SPI interface for HMC5983
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -77,7 +77,7 @@ public:
virtual ~HMC5883_SPI();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
@@ -91,7 +91,7 @@ HMC5883_SPI_interface(int bus)
}
HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */)
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
}
@@ -106,6 +106,7 @@ HMC5883_SPI::init()
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
@@ -148,10 +149,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg)
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default:
{
ret = -EINVAL;
}
default: {
ret = -EINVAL;
}
}
return ret;
src/drivers/hott/comms.cpp
+4 -3
View File
@@ -57,10 +57,11 @@ open_uart(const char *device)
if (uart < 0) {
err(1, "ERR: opening %s", device);
}
/* Back up the original uart configuration to restore it after exit */
/* Back up the original uart configuration to restore it after exit */
int termios_state;
struct termios uart_config_original;
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
err(1, "ERR: %s: %d", device, termios_state);
@@ -77,7 +78,7 @@ open_uart(const char *device)
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
src/drivers/hott/hott_sensors/hott_sensors.cpp
+14 -7
View File
@@ -104,15 +104,16 @@ int
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
{
static const int timeout_ms = 1000;
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
// XXX should this poll be inside the while loop???
if (poll(&fds, 1, timeout_ms) > 0) {
int i = 0;
bool stop_byte_read = false;
while (true) {
read(uart, &buffer[i], sizeof(buffer[i]));
@@ -121,14 +122,17 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
*size = ++i;
return OK;
}
// XXX can some other field not have the STOP BYTE value?
if (buffer[i] == STOP_BYTE) {
*id = buffer[1];
stop_byte_read = true;
}
i++;
}
}
return ERROR;
}
@@ -156,6 +160,7 @@ hott_sensors_thread_main(int argc, char *argv[])
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Open fail, exiting.");
thread_running = false;
@@ -166,6 +171,7 @@ hott_sensors_thread_main(int argc, char *argv[])
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
while (!thread_should_exit) {
// Currently we only support a General Air Module sensor.
build_gam_request(&buffer[0], &size);
@@ -179,6 +185,7 @@ hott_sensors_thread_main(int argc, char *argv[])
// Determine which moduel sent it and process accordingly.
if (id == GAM_SENSOR_ID) {
publish_gam_message(buffer);
} else {
warnx("Unknown sensor ID: %d", id);
}
@@ -210,11 +217,11 @@ hott_sensors_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
hott_sensors_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
hott_sensors_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+13 -6
View File
@@ -90,7 +90,7 @@ recv_req_id(int uart, uint8_t *id)
static const int timeout_ms = 1000; // TODO make it a define
uint8_t mode;
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
@@ -106,6 +106,7 @@ recv_req_id(int uart, uint8_t *id)
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
} else {
warnx("UART timeout on TX/RX port");
return ERROR;
@@ -120,6 +121,7 @@ send_data(int uart, uint8_t *buffer, size_t size)
usleep(POST_READ_DELAY_IN_USECS);
uint16_t checksum = 0;
for (size_t i = 0; i < size; i++) {
if (i == size - 1) {
/* Set the checksum: the first uint8_t is taken as the checksum. */
@@ -167,6 +169,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
@@ -178,6 +181,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
size_t size = 0;
uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) {
// Listen for and serve poll from the receiver.
if (recv_req_id(uart, &id) == OK) {
@@ -190,9 +194,11 @@ hott_telemetry_thread_main(int argc, char *argv[])
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
break;
case GAM_SENSOR_ID:
build_gam_response(buffer, &size);
break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;
@@ -202,6 +208,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
}
send_data(uart, buffer, size);
} else {
connected = false;
warnx("syncing");
@@ -236,11 +243,11 @@ hott_telemetry_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
hott_telemetry_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
hott_telemetry_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/drivers/hott/messages.cpp
+20 -16
View File
@@ -69,7 +69,7 @@ static bool _home_position_set = false;
static double _home_lat = 0.0d;
static double _home_lon = 0.0d;
void
void
init_sub_messages(void)
{
_battery_sub = orb_subscribe(ORB_ID(battery_status));
@@ -80,7 +80,7 @@ init_sub_messages(void)
_esc_sub = orb_subscribe(ORB_ID(esc_status));
}
void
void
init_pub_messages(void)
{
}
@@ -122,12 +122,13 @@ publish_gam_message(const uint8_t *buffer)
/* announce the esc if needed, just publish else */
if (_esc_pub != nullptr) {
orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
} else {
_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
}
}
void
void
build_eam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
@@ -147,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
msg.start = START_BYTE;
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
@@ -170,7 +171,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
memcpy(buffer, &msg, *size);
}
void
void
build_gam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the ESC Status values */
@@ -185,7 +186,7 @@ build_gam_response(uint8_t *buffer, size_t *size)
msg.start = START_BYTE;
msg.gam_sensor_id = GAM_SENSOR_ID;
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
msg.temperature2 = 20; // 0 deg. C.
@@ -205,7 +206,7 @@ build_gam_response(uint8_t *buffer, size_t *size)
memcpy(buffer, &msg, *size);
}
void
void
build_gps_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
@@ -213,7 +214,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
@@ -241,12 +242,13 @@ build_gps_response(uint8_t *buffer, size_t *size)
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat))*1e-7d;
/* Set the N or S specifier */
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat)) * 1e-7d;
/* Set the N or S specifier */
msg.latitude_ns = 0;
if (lat < 0) {
msg.latitude_ns = 1;
lat = abs(lat);
@@ -265,32 +267,34 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon))*1e-7d;
double lon = ((double)(gps.lon)) * 1e-7d;
/* Set the E or W specifier */
msg.longitude_ew = 0;
if (lon < 0) {
msg.longitude_ew = 1;
lon = abs(lon);
}
convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
uint16_t lon_min = (uint16_t)(deg * 100 + min);
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
uint16_t lon_sec = (uint16_t)(sec);
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) _home_sub postion report */
bool updated;
orb_check(_home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
src/drivers/hott/messages.h
+52 -52
View File
@@ -125,63 +125,63 @@ struct eam_module_msg {
#define GAM_SENSOR_TEXT_ID 0xd0
struct gam_module_msg {
uint8_t start; /**< Start byte */
uint8_t gam_sensor_id; /**< GAM sensor id */
uint8_t warning_beeps;
uint8_t sensor_text_id;
uint8_t alarm_invers1;
uint8_t alarm_invers2;
uint8_t cell1; /**< Lipo cell voltages. Not supported. */
uint8_t cell2;
uint8_t cell3;
uint8_t cell4;
uint8_t cell5;
uint8_t cell6;
uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */
uint8_t batt1_H;
uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */
uint8_t batt2_H;
uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */
uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */
uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */
/**< Graphical display ranges: 0 25% 50% 75% 100% */
uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */
uint8_t fuel_ml_H;
uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
uint8_t rpm_H;
uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */
uint8_t altitude_H;
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
uint8_t climbrate_H;
uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
uint8_t current_L; /**< Current in 0.1A steps */
uint8_t current_H;
uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */
uint8_t main_voltage_H;
uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */
uint8_t batt_cap_H;
uint8_t speed_L; /**< Speed in km/h */
uint8_t speed_H;
uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */
uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
uint8_t rpm2_H;
uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */
uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
uint8_t version;
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
uint8_t start; /**< Start byte */
uint8_t gam_sensor_id; /**< GAM sensor id */
uint8_t warning_beeps;
uint8_t sensor_text_id;
uint8_t alarm_invers1;
uint8_t alarm_invers2;
uint8_t cell1; /**< Lipo cell voltages. Not supported. */
uint8_t cell2;
uint8_t cell3;
uint8_t cell4;
uint8_t cell5;
uint8_t cell6;
uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */
uint8_t batt1_H;
uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */
uint8_t batt2_H;
uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */
uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */
uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */
/**< Graphical display ranges: 0 25% 50% 75% 100% */
uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */
uint8_t fuel_ml_H;
uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
uint8_t rpm_H;
uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */
uint8_t altitude_H;
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
uint8_t climbrate_H;
uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
uint8_t current_L; /**< Current in 0.1A steps */
uint8_t current_H;
uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */
uint8_t main_voltage_H;
uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */
uint8_t batt_cap_H;
uint8_t speed_L; /**< Speed in km/h */
uint8_t speed_H;
uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */
uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
uint8_t rpm2_H;
uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */
uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
uint8_t version;
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
};
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8a
#define GPS_SENSOR_TEXT_ID 0xa0
/**
/**
* The GPS sensor message
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
*/
struct gps_module_msg {
struct gps_module_msg {
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< 0…= warning beeps */
@@ -191,19 +191,19 @@ struct gps_module_msg {
uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */
uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */
uint8_t latitude_ns; /**< 000 = N = 48°39’988 */
uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */
uint8_t latitude_min_H; /**< 018 18 = 0x12 */
uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */
uint8_t latitude_sec_H; /**< 016 3 = 0x03 */
uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */
uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */
uint8_t longitude_min_H; /**< 003 3 = 0x03 */
uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */
uint8_t longitude_sec_H; /**< 004 36 = 0x24 */
uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */
uint8_t distance_H; /**< 036 35 = /distance high byte */
uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */

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