yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge ROS into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-02-03 20:07:55 +01:00
parent 3e7faa6018 d441d38677
commit dc46736ead
189 changed files with 10102 additions and 1865 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@@ -17,6 +17,7 @@
.~lock.*
Archives/*
Build/*
build/*
core
cscope.out
Firmware.sublime-workspace
@@ -38,6 +39,10 @@ tags
.pydevproject
.ropeproject
*.orig
src/modules/uORB/topics/*
src/platforms/nuttx/px4_messages/*
src/platforms/ros/px4_messages/*
Firmware.zip
unittests/build
*.generated.h
.vagrant

6
.gitmodules vendored
View File

@@ -7,6 +7,12 @@
[submodule "uavcan"]
path = src/lib/uavcan
url = git://github.com/pavel-kirienko/uavcan.git
[submodule "Tools/genmsg"]
path = Tools/genmsg
url = https://github.com/ros/genmsg.git
[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

2
.travis.yml
View File

@@ -12,7 +12,7 @@ before_script:
- sudo apt-get install s3cmd grep zip mailutils
# General toolchain dependencies
- sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
- sudo apt-get install python-serial python-argparse
- sudo apt-get install python-serial python-argparse python-empy
- sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake
- pushd .
- cd ~

289
CMakeLists.txt Normal file
View File

@@ -0,0 +1,289 @@
cmake_minimum_required(VERSION 2.8.3)
project(px4)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-D__PX4_ROS)
add_definitions(-D__EXPORT=)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
cmake_modules
gazebo_msgs
sensor_msgs
mav_msgs
)
find_package(Eigen REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
rc_channels.msg
vehicle_attitude.msg
vehicle_attitude_setpoint.msg
manual_control_setpoint.msg
actuator_controls.msg
actuator_controls_0.msg
actuator_controls_virtual_mc.msg
vehicle_rates_setpoint.msg
mc_virtual_rates_setpoint.msg
vehicle_attitude.msg
vehicle_control_mode.msg
actuator_armed.msg
parameter_update.msg
vehicle_status.msg
vehicle_local_position.msg
position_setpoint.msg
position_setpoint_triplet.msg
vehicle_local_position_setpoint.msg
vehicle_global_velocity_setpoint.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
gazebo_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS src/include
LIBRARIES px4
CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
src/platforms
src/platforms/ros/px4_messages
src/include
src/modules
src/
src/lib
${EIGEN_INCLUDE_DIRS}
)
## generate multiplatform wrapper headers
## note that the message header files are generated as in any ROS project with generate_messages()
set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
set(MULTIPLATFORM_PREFIX px4_)
add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py
-d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
## Declare a cpp library
add_library(px4
src/platforms/ros/px4_ros_impl.cpp
src/platforms/ros/perf_counter.cpp
src/platforms/ros/geo.cpp
src/lib/mathlib/math/Limits.cpp
src/modules/systemlib/circuit_breaker.cpp
)
add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
target_link_libraries(px4
${catkin_LIBRARIES}
)
## Declare a test publisher
add_executable(publisher
src/examples/publisher/publisher_main.cpp
src/examples/publisher/publisher_example.cpp)
add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
target_link_libraries(publisher
${catkin_LIBRARIES}
px4
)
## Declare a test subscriber
add_executable(subscriber
src/examples/subscriber/subscriber_main.cpp
src/examples/subscriber/subscriber_example.cpp)
add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
target_link_libraries(subscriber
${catkin_LIBRARIES}
px4
)
## MC Attitude Control
add_executable(mc_att_control
src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(mc_att_control
${catkin_LIBRARIES}
px4
)
## MC Position Control
add_executable(mc_pos_control
src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(mc_pos_control
${catkin_LIBRARIES}
px4
)
## Attitude Estimator dummy
add_executable(attitude_estimator
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(attitude_estimator
${catkin_LIBRARIES}
px4
)
## Position Estimator dummy
add_executable(position_estimator
src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(position_estimator
${catkin_LIBRARIES}
px4
)
## Manual input
add_executable(manual_input
src/platforms/ros/nodes/manual_input/manual_input.cpp)
add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(manual_input
${catkin_LIBRARIES}
px4
)
## Multicopter Mixer dummy
add_executable(mc_mixer
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(mc_mixer
${catkin_LIBRARIES}
px4
)
## Commander
add_executable(commander
src/platforms/ros/nodes/commander/commander.cpp)
add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(commander
${catkin_LIBRARIES}
px4
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

37
Makefile
View File

@@ -104,13 +104,13 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
all: checksubmodules $(DESIRED_FIRMWARES)
all: $(DESIRED_FIRMWARES)
#
# Copy FIRMWARES into the image directory.
#
# XXX copying the .bin files is a hack to work around the PX4IO uploader
# not supporting .px4 files, and it should be deprecated onced that
# XXX copying the .bin files is a hack to work around the PX4IO uploader
# not supporting .px4 files, and it should be deprecated onced that
# is taken care of.
#
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
.PHONY: $(FIRMWARES)
$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checksubmodules generateuorbtopicheaders
@$(ECHO) %%%%
@$(ECHO) %%%% Building $(config) in $(work_dir)
@$(ECHO) %%%%
@@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
# Build the NuttX export archives.
#
# Note that there are no explicit dependencies extended from these
# archives. If NuttX is updated, the user is expected to rebuild the
# archives. If NuttX is updated, the user is expected to rebuild the
# archives/build area manually. Likewise, when the 'archives' target is
# invoked, all archives are always rebuilt.
#
@@ -226,6 +226,29 @@ updatesubmodules:
$(Q) (git submodule init)
$(Q) (git submodule update)
MSG_DIR = $(PX4_BASE)msg
UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb
MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb
TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
MULTIPLATFORM_PREFIX = px4_
TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary
GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
.PHONY: generateuorbtopicheaders
generateuorbtopicheaders:
@$(ECHO) "Generating uORB topic headers"
$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR))
@$(ECHO) "Generating multiplatform uORB topic wrapper headers"
$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
-d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX))
# clean up temporary files
$(Q) (rm -r $(TOPICHEADER_TEMP_DIR))
#
# Testing targets
#
@@ -237,12 +260,12 @@ testbuild:
# Unittest targets. Builds and runs the host-level
# unit tests.
.PHONY: tests
tests:
tests: generateuorbtopicheaders
$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
#
# Cleanup targets. 'clean' should remove all built products and force
# a complete re-compilation, 'distclean' should remove everything
# a complete re-compilation, 'distclean' should remove everything
# that's generated leaving only files that are in source control.
#
.PHONY: clean

15
ROMFS/px4fmu_common/init.d/rc.mc_apps
View File

@@ -8,8 +8,19 @@ attitude_estimator_ekf start
#ekf_att_pos_estimator start
position_estimator_inav start
mc_att_control start
mc_pos_control start
if mc_att_control start
then
else
# try the multiplatform version
mc_att_control_m start
fi
if mc_pos_control start
then
else
# try the multiplatform version
mc_pos_control_m start
fi
#
# Start Land Detector

1
Tools/gencpp Submodule

Submodule Tools/gencpp added at 26a86f04bc

1
Tools/genmsg Submodule

Submodule Tools/genmsg added at 72f0383f0e

159
Tools/px_generate_uorb_topic_headers.py Executable file
View File

@@ -0,0 +1,159 @@
#!/usr/bin/env python
#############################################################################
#
# Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#############################################################################
"""
px_generate_uorb_topic_headers.py
Generates c/cpp header files for uorb topics from .msg (ROS syntax)
message files
"""
from __future__ import print_function
import os
import shutil
import filecmp
import argparse
import genmsg.template_tools
__author__ = "Thomas Gubler"
__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
__license__ = "BSD"
__email__ = "thomasgubler@gmail.com"
msg_template_map = {'msg.h.template': '@NAME@.h'}
srv_template_map = {}
incl_default = ['std_msgs:./msg/std_msgs']
package = 'px4'
def convert_file(filename, outputdir, templatedir, includepath):
"""
Converts a single .msg file to a uorb header
"""
print("Generating headers from {0}".format(filename))
genmsg.template_tools.generate_from_file(filename,
package,
outputdir,
templatedir,
includepath,
msg_template_map,
srv_template_map)
def convert_dir(inputdir, outputdir, templatedir):
"""
Converts all .msg files in inputdir to uORB header files
"""
includepath = incl_default + [':'.join([package, inputdir])]
for f in os.listdir(inputdir):
fn = os.path.join(inputdir, f)
if os.path.isfile(fn):
convert_file(
fn,
outputdir,
templatedir,
includepath)
def copy_changed(inputdir, outputdir, prefix=''):
"""
Copies files from inputdir to outputdir if they don't exist in
ouputdir or if their content changed
"""
# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)
for f in os.listdir(inputdir):
fni = os.path.join(inputdir, f)
if os.path.isfile(fni):
# Check if f exists in outpoutdir, copy the file if not
fno = os.path.join(outputdir, prefix + f)
if not os.path.isfile(fno):
shutil.copy(fni, fno)
print("{0}: new header file".format(f))
continue
# The file exists in inputdir and outputdir
# only copy if contents do not match
if not filecmp.cmp(fni, fno):
shutil.copy(fni, fno)
print("{0}: updated".format(f))
continue
print("{0}: unchanged".format(f))
def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix):
"""
Converts all .msg files in inputdir to uORB header files
Unchanged existing files are not overwritten.
"""
# Create new headers in temporary output directory
convert_dir(inputdir, temporarydir, templatedir)
# Copy changed headers from temporary dir to output dir
copy_changed(temporarydir, outputdir, prefix)
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description='Convert msg files to uorb headers')
parser.add_argument('-d', dest='dir', help='directory with msg files')
parser.add_argument('-f', dest='file',
help="files to convert (use only without -d)",
nargs="+")
parser.add_argument('-e', dest='templatedir',
help='directory with template files',)
parser.add_argument('-o', dest='outputdir',
help='output directory for header files')
parser.add_argument('-t', dest='temporarydir',
help='temporary directory')
parser.add_argument('-p', dest='prefix', default='',
help='string added as prefix to the output file '
' name when converting directories')
args = parser.parse_args()
if args.file is not None:
for f in args.file:
convert_file(
f,
args.outputdir,
args.templatedir,
incl_default)
elif args.dir is not None:
convert_dir_save(
args.dir,
args.outputdir,
args.templatedir,
args.temporarydir,
args.prefix)

57
Tools/ros/docker/px4-ros-full/Dockerfile Normal file
View File

@@ -0,0 +1,57 @@
#
# PX4 full ROS container
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
FROM ubuntu:14.04.1
MAINTAINER Andreas Antener <andreas@uaventure.com>
# Install basics
## Use the "noninteractive" debconf frontend
ENV DEBIAN_FRONTEND noninteractive
RUN apt-get update \
&& apt-get -y install wget git mercurial
# Main ROS Setup
# Following http://wiki.ros.org/indigo/Installation/Ubuntu
# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install
## add ROS repositories and keys
## install main ROS pacakges
RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \
&& wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \
&& apt-get update \
&& apt-get -y install ros-indigo-desktop-full
RUN rosdep init \
&& rosdep update
## setup environment variables
RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
## get rosinstall
RUN apt-get -y install python-rosinstall
## additional dependencies
RUN apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
## install drcsim
RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \
&& wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \
&& apt-get update \
&& apt-get -y install drcsim
# Install x11-utils to get xdpyinfo, for X11 display debugging
# mesa-utils provides glxinfo, handy for understanding the 3D support
RUN apt-get -y install x11-utils mesa-utils
# Some QT-Apps/Gazebo don't not show controls without this
ENV QT_X11_NO_MITSHM 1
# FIXME: this doesn't work when building from vagrant
COPY scripts/setup-workspace.sh /root/scripts/
RUN chmod +x -R /root/scripts/*
RUN chown -R root:root /root/scripts/*
CMD ["/usr/bin/xterm"]

12
Tools/ros/docker/px4-ros-full/README.md Normal file
View File

@@ -0,0 +1,12 @@
# PX4 ROS #
Full desktop ROS container.
License: according to LICENSE.md in the root directory of the PX4 Firmware repository
**TODO:**
- use https://github.com/phusion/baseimage-docker as base
- add user, best synced with host
- configure ssh to work with vagrant out of the box

45
Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh Normal file
View File

@@ -0,0 +1,45 @@
#!/bin/sh
#
# Create workspace at current location and fetch source repositories
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
WDIR=`pwd`
WORKSPACE=$WDIR/catkin_ws
# Setup workspace
mkdir -p $WORKSPACE/src
cd $WORKSPACE/src
catkin_init_workspace
cd $WORKSPACE
catkin_make
echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc
# PX4 Firmware
cd $WORKSPACE/src
git clone https://github.com/PX4/Firmware.git \
&& cd Firmware \
&& git checkout ros
# euroc simulator
cd $WORKSPACE/src
git clone https://github.com/PX4/euroc_simulator.git \
&& cd euroc_simulator \
&& git checkout px4_nodes
# mav comm
cd $WORKSPACE/src
git clone https://github.com/PX4/mav_comm.git
# glog catkin
cd $WORKSPACE/src
git clone https://github.com/ethz-asl/glog_catkin.git
# catkin simple
cd $WORKSPACE/src
git clone https://github.com/catkin/catkin_simple.git
cd $WORKSPACE
catkin_make

41
Tools/ros/px4_ros_installation_ubuntu.sh Executable file
View File

@@ -0,0 +1,41 @@
#!/bin/sh
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
# main ROS Setup
# following http://wiki.ros.org/indigo/Installation/Ubuntu
# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install
# run this file with . ./px4_ros_setup_ubuntu.sh
## add ROS repository
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
## add key
wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
sudo apt-key add -
## Install main ROS pacakges
sudo apt-get update
sudo apt-get -y install ros-indigo-desktop-full
sudo rosdep init
rosdep update
## Setup environment variables
echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
. ~/.bashrc
# get rosinstall
sudo apt-get -y install python-rosinstall
# additional dependencies
sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
## drcsim setup (for models)
### add osrf repository
sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list'
### add key
wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
### install drcsim
sudo apt-get update
sudo apt-get -y install drcsim

9
Tools/ros/px4_workspace_create.sh Executable file
View File

@@ -0,0 +1,9 @@
#!/bin/sh
# this script creates a catkin_ws in the current folder
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

33
Tools/ros/px4_workspace_setup.sh Executable file
View File

@@ -0,0 +1,33 @@
#!/bin/bash
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
# run this script from the root of your catkin_ws
source devel/setup.bash
cd src
# PX4 Firmware
git clone https://github.com/PX4/Firmware.git
cd Firmware
git checkout ros
cd ..
# euroc simulator
git clone https://github.com/PX4/euroc_simulator.git
cd euroc_simulator
git checkout px4_nodes
cd ..
# mav comm
git clone https://github.com/PX4/mav_comm.git
# glog catkin
git clone https://github.com/ethz-asl/glog_catkin.git
# catkin simple
git clone https://github.com/catkin/catkin_simple.git
# drcsim (for scenery and models)
cd ..
catkin_make

58
Tools/ros/vagrant/docker-host-base/Vagrantfile vendored Normal file
View File

@@ -0,0 +1,58 @@
# -*- mode: ruby -*-
# vi: set ft=ruby :
#
# Vagrantfile to create docker-host-base
#
# Maintainer: Andreas Antener <andreas@uaventure.com>
#
# After build, do "vagrant package --base docker-host-base" to package,
# and import as box: "vagrant box add --name uaventure/docker-host-base package.box"
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
Vagrant.configure(2) do |config|
config.vm.box = "ubuntu/trusty64"
config.vm.define "docker-host-base"
config.vm.provider "virtualbox" do |vb|
vb.name = "docker-host-base"
vb.gui = true
vb.memory = "1024"
end
config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default"
config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc"
config.vm.provision "shell", inline: <<-SHELL
# Update and install apps
export DEBIAN_FRONTEND=noninteractive
sudo apt-get update
sudo apt-get upgrade -y
sudo apt-get install -y --no-install-recommends ubuntu-desktop
sudo apt-get install -y gnome-terminal unity-lens-applications
# Reset the ssh key (because vagrant regenerates it during provisioning)
sudo wget --no-check-certificate https://raw.github.com/mitchellh/vagrant/master/keys/vagrant.pub -O /home/vagrant/.ssh/authorized_keys
sudo chmod 0700 /home/vagrant/.ssh
sudo chmod 0600 /home/vagrant/.ssh/authorized_keys
sudo chown -R vagrant /home/vagrant/.ssh
# Copy docker config
sudo mv /home/vagrant/docker-default /etc/default/docker
# Enable autologin so docker can start GUI apps
sudo echo "autologin-user=vagrant" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
sudo echo "autologin-user-timeout=0" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
# X session RC
chmod +x /home/vagrant/.xsessionrc
SHELL
config.vm.provision "docker"
# Shutdown after provisioning. "vagrant halt" doesn't recognize the original ssh key anymore
# and would just kill the VM. This might lead to FS inconsistencies (e.g. in the docker DB).
config.vm.provision "shell", inline: "sudo shutdown -h now"
end

29
Tools/ros/vagrant/docker-host-base/config/docker-default Normal file
View File

@@ -0,0 +1,29 @@
#
# Default config for docker /etc/default/docker
# Copied from a provisioned vagrant box
#
# Modifications:
# - listen to TCP port
# - removing deprecated "-r=true" option which apparently doesn't work anymore
# > use restart policies for specific containers if necessary
#
# Docker Upstart and SysVinit configuration file
# Customize location of Docker binary (especially for development testing).
#DOCKER="/usr/local/bin/docker"
# Use DOCKER_OPTS to modify the daemon startup options.
#DOCKER_OPTS="--dns 8.8.8.8 --dns 8.8.4.4"
# If you need Docker to use an HTTP proxy, it can also be specified here.
#export http_proxy="http://127.0.0.1:3128/"
# This is also a handy place to tweak where Docker's temporary files go.
#export TMPDIR="/mnt/bigdrive/docker-tmp"
# Expose TCP port in addition to socket
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375"

6
Tools/ros/vagrant/docker-host-base/config/xsessionrc Normal file
View File

@@ -0,0 +1,6 @@
#!/bin/sh
#
# Disable X access control so we can easily start GUI apps
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
xhost +

38
Tools/ros/vagrant/docker-host/Vagrantfile vendored Normal file
View File

@@ -0,0 +1,38 @@
# -*- mode: ruby -*-
# vi: set ft=ruby :
#
# Actual docker host VM to run.
#
# Maintainer: Andreas Antener <andreas@uaventure.com>
#
# To add local docker images into the docker host, configure your local
# docker client to control the docker daemon on the running "docker-host" VM.
# The box ("docker-host-base") configures docker to listen on any IP on port 2375.
# You can then load an existing image, e.g.:
# "docker load -i container-image.tar"
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
Vagrant.configure(2) do |config|
config.vm.box = "uaventure/docker-host-base"
config.vm.define "docker-host"
config.vm.provider "virtualbox" do |vb|
vb.name = "docker-host"
vb.gui = true
vb.memory = "4096"
vb.cpus = 2
vb.customize ["modifyvm", :id, "--graphicscontroller", "vboxvga"]
vb.customize ["modifyvm", :id, "--accelerate3d", "on"]
vb.customize ["modifyvm", :id, "--ioapic", "on"]
vb.customize ["modifyvm", :id, "--vram", "128"]
vb.customize ["modifyvm", :id, "--hwvirtex", "on"]
end
config.vm.network "private_network", ip: "192.168.59.104"
# TBD: would it be better to provision docker here instead of in the base box?
#config.vm.provision "docker"
end

61
Tools/ros/vagrant/px4-ros/Vagrantfile vendored Normal file
View File

@@ -0,0 +1,61 @@
# -*- mode: ruby -*-
# vi: set ft=ruby :
#
# Boot docker SITL environment
#
# Maintainer: Andreas Antener <andreas@uaventure.com>
#
# "vagrant up" will build the images. Should eventually start "xterm" from within the docker container.
#
# Notes:
# (will change, need proper docs)
#
# Build with multiple dependent docker containers:
# Use the "--no-parallel" option so the containers will be built/started in order.
# e.g.: "vagrant up --no-parallel"
#
# Running apps directly:
# "vagrant docker-run ros -- <cmd>"
# Attention: will loose all data when stopped, vagrant runs docker always with "--rm"
#
# TODO
# - do not run the docker container with "--rm" (vagrant default). is that even possible?
# - maybe map a local working directory to compile stuff without loosing it in side the docker container
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
Vagrant.configure(2) do |config|
# Configure docker host
config.vm.provider "docker" do |d|
d.vagrant_machine = "docker-host"
d.vagrant_vagrantfile = "../docker-host/Vagrantfile"
end
# Configure docker apps to run
config.vm.define "ros" do |app|
app.vm.provider "docker" do |d|
d.name = "ros"
d.image = "uaventure/px4-ros-full"
#d.build_dir = "../../docker/px4-ros-full"
#d.build_args = ["-t=uaventure/px4-ros-full"]
# Share docker host x11 socket
# Run privileged to support 3d acceleration
d.volumes = [
"/tmp/.X11-unix:/tmp/.X11-unix:ro"
]
d.create_args = ["--privileged"]
# TODO: get display number from host system
d.env = {
"DISPLAY" => ":0"
}
d.remains_running = true
d.cmd = ["xterm"]
#d.has_ssh = true
end
end
end

14
launch/ardrone.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<include file="$(find px4)/launch/multicopter_x.launch" />
<group ns="px4_multicopter">
<param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.0" />
<param name="MPC_XY_VEL_P" type="double" value="0.01" />
<param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
</group>
</launch>

8
launch/example.launch Normal file
View File

@@ -0,0 +1,8 @@
<launch>
<group ns="px4_example">
<node pkg="px4" name="subscriber" type="subscriber"/>
<node pkg="px4" name="publisher" type="publisher"/>
</group>
</launch>

6
launch/gazebo_ardrone_empty_world.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" />
<include file="$(find px4)/launch/ardrone.launch" />
</launch>

6
launch/gazebo_ardrone_house_world.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" />
<include file="$(find px4)/launch/ardrone.launch" />
</launch>

6
launch/gazebo_iris_empty_world.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" />
<include file="$(find px4)/launch/iris.launch" />
</launch>

6
launch/gazebo_iris_house_world.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<include file="$(find mav_gazebo)/launch/iris_house_world_with_joy.launch" />
<include file="$(find px4)/launch/iris.launch" />
</launch>

6
launch/gazebo_iris_outdoor_world.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" />
<include file="$(find px4)/launch/iris.launch" />
</launch>

15
launch/iris.launch Normal file
View File

@@ -0,0 +1,15 @@
<launch>
<include file="$(find px4)/launch/multicopter_w.launch" />
<group ns="px4_multicopter">
<param name="mixer" type="string" value="i" />
<param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.0" />
<param name="MPC_XY_VEL_P" type="double" value="0.01" />
<param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
</group>
</launch>

15
launch/multicopter.launch Normal file
View File

@@ -0,0 +1,15 @@
<launch>
<group ns="px4_multicopter">
<node pkg="joy" name="joy_node" type="joy_node"/>
<node pkg="px4" name="manual_input" type="manual_input"/>
<node pkg="px4" name="commander" type="commander"/>
<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
<node pkg="px4" name="position_estimator" type="position_estimator"/>
<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
</group>
</launch>

9
launch/multicopter_w.launch Normal file
View File

@@ -0,0 +1,9 @@
<launch>
<include file="$(find px4)/launch/multicopter.launch" />
<group ns="px4_multicopter">
<param name="mixer" type="string" value="w" />
</group>
</launch>

9
launch/multicopter_x.launch Normal file
View File

@@ -0,0 +1,9 @@
<launch>
<include file="$(find px4)/launch/multicopter.launch" />
<group ns="px4_multicopter">
<param name="mixer" type="string" value="x" />
</group>
</launch>

1
makefiles/config_px4fmu-v1_default.mk
View File

@@ -111,6 +111,7 @@ MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
MODULES += platforms/nuttx
#
# Demo apps

1
makefiles/config_px4fmu-v2_default.mk
View File

@@ -116,6 +116,7 @@ MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
MODULES += platforms/nuttx
#
# OBC challenge

170
makefiles/config_px4fmu-v2_multiplatform.mk Normal file
View File

@@ -0,0 +1,170 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS, copy the px4iov2 firmware into
# the ROMFS if it's available
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4fmu
MODULES += drivers/px4io
MODULES += drivers/boards/px4fmu-v2
MODULES += drivers/rgbled
MODULES += drivers/mpu6000
MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
# MODULES += drivers/sf0x
MODULES += drivers/ll40ls
# MODULES += drivers/trone
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
# MODULES += drivers/blinkm
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
MODULES += drivers/px4flow
#
# System commands
#
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/dumpfile
MODULES += systemcmds/ver
#
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
# MODULES += modules/uavcan
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
# MODULES += modules/mc_att_control
MODULES += modules/mc_att_control_multiplatform
MODULES += examples/subscriber
MODULES += examples/publisher
# MODULES += modules/mc_pos_control
MODULES += modules/mc_pos_control_multiplatform
MODULES += modules/vtol_att_control
#
# Logging
#
MODULES += modules/sdlog2
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
MODULES += platforms/nuttx
#
# OBC challenge
#
MODULES += modules/bottle_drop
#
# Demo apps
#
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
#MODULES += examples/px4_daemon_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
#MODULES += examples/fixedwing_control
# Hardware test
#MODULES += examples/hwtest
# Generate parameter XML file
GEN_PARAM_XML = 1
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main )

24
makefiles/config_px4fmu-v2_test.mk
View File

@@ -39,9 +39,30 @@ MODULES += systemcmds/top
MODULES += modules/sensors
#
# Testing modules
# System commands
#
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/dumpfile
MODULES += systemcmds/ver
#
# Example modules
#
MODULES += examples/matlab_csv_serial
MODULES += examples/subscriber
MODULES += examples/publisher
#
# Library modules
@@ -53,6 +74,7 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/conversion
MODULES += platforms/nuttx
#
# Example modules to test-build

6
makefiles/setup.mk
View File

@@ -37,13 +37,14 @@
# Some useful paths.
#
# Note that in general we always keep directory paths with the separator
# at the end, and join paths without explicit separators. This reduces
# at the end, and join paths without explicit separators. This reduces
# the number of duplicate slashes we have lying around in paths,
# and is consistent with joining the results of $(dir) and $(notdir).
#
export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/
export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/
export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/
export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/
export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/
export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
@@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/
export INCLUDE_DIRS := $(PX4_MODULE_SRC) \
$(PX4_MODULE_SRC)/modules/ \
$(PX4_INCLUDE_DIR) \
$(PX4_LIB_DIR)
$(PX4_LIB_DIR) \
$(PX4_PLATFORMS_DIR)
#
# Tools

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

@@ -128,7 +128,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
# Generic warnings
#

6
msg/actuator_armed.msg Normal file
View File

@@ -0,0 +1,6 @@
uint64 timestamp # Microseconds since system boot
bool armed # Set to true if system is armed
bool ready_to_arm # Set to true if system is ready to be armed
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
bool force_failsafe # Set to true if the actuators are forced to the failsafe position

5
msg/actuator_controls.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_0.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_1.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_2.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_3.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_virtual_fw.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

5
msg/actuator_controls_virtual_mc.msg Normal file
View File

@@ -0,0 +1,5 @@
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control

6
msg/fw_virtual_rates_setpoint.msg Normal file
View File

@@ -0,0 +1,6 @@
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1

44
msg/manual_control_setpoint.msg Normal file
View File

@@ -0,0 +1,44 @@
uint8 SWITCH_POS_NONE = 0 # switch is not mapped
uint8 SWITCH_POS_ON = 1 # switch activated (value = 1)
uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0)
uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1)
uint64 timestamp
# Any of the channels may not be available and be set to NaN
# to indicate that it does not contain valid data.
# The variable names follow the definition of the
# MANUAL_CONTROL mavlink message.
# The default range is from -1 to 1 (mavlink message -1000 to 1000)
# The range for the z variable is defined from 0 to 1. (The z field of
# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
float32 x # stick position in x direction -1..1
# in general corresponds to forward/back motion or pitch of vehicle,
# in general a positive value means forward or negative pitch and
# a negative value means backward or positive pitch
float32 y # stick position in y direction -1..1
# in general corresponds to right/left motion or roll of vehicle,
# in general a positive value means right or positive roll and
# a negative value means left or negative roll
float32 z # throttle stick position 0..1
# in general corresponds to up/down motion or thrust of vehicle,
# in general the value corresponds to the demanded throttle by the user,
# if the input is used for setting the setpoint of a vertical position
# controller any value > 0.5 means up and any value < 0.5 means down
float32 r # yaw stick/twist positon, -1..1
# in general corresponds to the righthand rotation around the vertical
# (downwards) axis of the vehicle
float32 flaps # flap position
float32 aux1 # default function: camera yaw / azimuth
float32 aux2 # default function: camera pitch / tilt
float32 aux3 # default function: camera trigger
float32 aux4 # default function: camera roll
float32 aux5 # default function: payload drop
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD

6
msg/mc_virtual_rates_setpoint.msg Normal file
View File

@@ -0,0 +1,6 @@
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1

1
msg/parameter_update.msg Normal file
View File

@@ -0,0 +1 @@
uint64 timestamp # time at which the latest parameter was updated

35
msg/position_setpoint.msg Normal file
View File

@@ -0,0 +1,35 @@
# this file is only used in the position_setpoint triple as a dependency
uint8 SETPOINT_TYPE_POSITION=0 # position setpoint
uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint
uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint
uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint
uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing
uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC)
uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
bool valid # true if setpoint is valid
uint8 type # setpoint type to adjust behavior of position controller
float32 x # local position setpoint in m in NED
float32 y # local position setpoint in m in NED
float32 z # local position setpoint in m in NED
bool position_valid # true if local position setpoint valid
float32 vx # local velocity setpoint in m/s in NED
float32 vy # local velocity setpoint in m/s in NED
float32 vz # local velocity setpoint in m/s in NED
bool velocity_valid # true if local velocity setpoint valid
float64 lat # latitude, in deg
float64 lon # longitude, in deg
float32 alt # altitude AMSL, in m
float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
bool yaw_valid # true if yaw setpoint valid
float32 yawspeed # yawspeed (only for multirotors, in rad/s)
bool yawspeed_valid # true if yawspeed setpoint valid
float32 loiter_radius # loiter radius (only for fixed wing), in m
int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW
float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints
float32 a_x # acceleration x setpoint
float32 a_y # acceleration y setpoint
float32 a_z # acceleration z setpoint
bool acceleration_valid # true if acceleration setpoint is valid/should be used
bool acceleration_is_force # interprete acceleration as force

8
msg/position_setpoint_triplet.msg Normal file
View File

@@ -0,0 +1,8 @@
# Global position setpoint triplet in WGS84 coordinates.
# This are the three next waypoints (or just the next two or one).
px4/position_setpoint previous
px4/position_setpoint current
px4/position_setpoint next
uint8 nav_state # report the navigation state

27
msg/rc_channels.msg Normal file
View File

@@ -0,0 +1,27 @@
int32 RC_CHANNELS_FUNCTION_MAX=19
uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
uint8 RC_CHANNELS_FUNCTION_ROLL=1
uint8 RC_CHANNELS_FUNCTION_PITCH=2
uint8 RC_CHANNELS_FUNCTION_YAW=3
uint8 RC_CHANNELS_FUNCTION_MODE=4
uint8 RC_CHANNELS_FUNCTION_RETURN=5
uint8 RC_CHANNELS_FUNCTION_POSCTL=6
uint8 RC_CHANNELS_FUNCTION_LOITER=7
uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
uint8 RC_CHANNELS_FUNCTION_ACRO=9
uint8 RC_CHANNELS_FUNCTION_FLAPS=10
uint8 RC_CHANNELS_FUNCTION_AUX_1=11
uint8 RC_CHANNELS_FUNCTION_AUX_2=12
uint8 RC_CHANNELS_FUNCTION_AUX_3=13
uint8 RC_CHANNELS_FUNCTION_AUX_4=14
uint8 RC_CHANNELS_FUNCTION_AUX_5=15
uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
uint64 timestamp # Timestamp in microseconds since boot time
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[19] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[19] function # Functions mapping
uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout

95
msg/templates/px4/ros/msg.h.template Normal file
View File

@@ -0,0 +1,95 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# This file generates the multiplatform wrapper
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - md5sum (String) MD5Sum of the .msg specification
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
import gencpp
cpp_class = 'px4_%s'%spec.short_name
native_type = spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include "px4/@(topic_name).h"
#include "platforms/px4_message.h"
@##############################
@# Class
@##############################
namespace px4
{
class @(cpp_class) :
public PX4Message<@(native_type)>
{
public:
@(cpp_class)() :
PX4Message<@(native_type)>()
{}
@(cpp_class)(@(native_type) msg) :
PX4Message<@(native_type)>(msg)
{}
~@(cpp_class)() {}
static PX4TopicHandle handle() {return "@(topic_name)";}
};
};

95
msg/templates/px4/uorb/msg.h.template Normal file
View File

@@ -0,0 +1,95 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# This file generates the multiplatform wrapper
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - md5sum (String) MD5Sum of the .msg specification
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
import gencpp
cpp_class = 'px4_%s'%spec.short_name
native_type = '%s_s'%spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include <uORB/topics/@(topic_name).h>
#include "platforms/px4_message.h"
@##############################
@# Class
@##############################
namespace px4
{
class __EXPORT @(cpp_class) :
public PX4Message<@(native_type)>
{
public:
@(cpp_class)() :
PX4Message<@(native_type)>()
{}
@(cpp_class)(@(native_type) msg) :
PX4Message<@(native_type)>(msg)
{}
~@(cpp_class)() {}
static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
};
};

175
msg/templates/uorb/msg.h.template Normal file
View File

@@ -0,0 +1,175 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - md5sum (String) MD5Sum of the .msg specification
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
import gencpp
uorb_struct = '%s_s'%spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include <stdint.h>
#include <uORB/uORB.h>
@##############################
@# Includes for dependencies
@##############################
@{
for field in spec.parsed_fields():
if (not field.is_builtin):
if (not field.is_header):
(package, name) = genmsg.names.package_resource_name(field.base_type)
package = package or spec.package # convert '' to package
print('#include <uORB/topics/%s.h>'%(name))
}@
@# Constants c style
#ifndef __cplusplus
@[for constant in spec.constants]@
#define @(constant.name) @(int(constant.val))
@[end for]
#endif
/**
* @@addtogroup topics
* @@{
*/
@##############################
@# Main struct of message
@##############################
@{
type_map = {'int8': 'int8_t',
'int16': 'int16_t',
'int32': 'int32_t',
'int64': 'int64_t',
'uint8': 'uint8_t',
'uint16': 'uint16_t',
'uint32': 'uint32_t',
'uint64': 'uint64_t',
'float32': 'float',
'float64': 'double',
'bool': 'bool',
'fence_vertex': 'fence_vertex',
'position_setpoint': 'position_setpoint'}
# Function to print a standard ros type
def print_field_def(field):
type = field.type
# detect embedded types
sl_pos = type.find('/')
type_appendix = ''
type_prefix = ''
if (sl_pos >= 0):
type = type[sl_pos + 1:]
type_prefix = 'struct '
type_appendix = '_s'
# detect arrays
a_pos = type.find('[')
array_size = ''
if (a_pos >= 0):
# field is array
array_size = type[a_pos:]
type = type[:a_pos]
if type in type_map:
# need to add _t: int8 --> int8_t
type_px4 = type_map[type]
else:
raise Exception("Type {0} not supported, add to to template file!".format(type))
print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
}
#ifdef __cplusplus
@#class @(uorb_struct) {
struct __EXPORT @(uorb_struct) {
@#public:
#else
struct @(uorb_struct) {
#endif
@{
# loop over all fields and print the type and name
for field in spec.parsed_fields():
if (not field.is_header):
print_field_def(field)
}@
#ifdef __cplusplus
@# Constants again c++-ified
@{
for constant in spec.constants:
type = constant.type
if type in type_map:
# need to add _t: int8 --> int8_t
type_px4 = type_map[type]
else:
raise Exception("Type {0} not supported, add to to template file!".format(type))
print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
}
#endif
};
/**
* @@}
*/
/* register this as object request broker structure */
ORB_DECLARE(@(topic_name));

18
msg/vehicle_attitude.msg Normal file
View File

@@ -0,0 +1,18 @@
# This is similar to the mavlink message ATTITUDE, but for onboard use */
uint64 timestamp # in microseconds since system start
# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional
float32 roll # Roll angle (rad, Tait-Bryan, NED)
float32 pitch # Pitch angle (rad, Tait-Bryan, NED)
float32 yaw # Yaw angle (rad, Tait-Bryan, NED)
float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED)
float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED)
float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED)
float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED)
float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED)
float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED)
float32[3] rate_offsets # Offsets of the body angular rates from zero
float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED)
float32[4] q # Quaternion (NED)
float32[3] g_comp # Compensated gravity vector
bool R_valid # Rotation matrix valid
bool q_valid # Quaternion valid

21
msg/vehicle_attitude_setpoint.msg Normal file
View File

@@ -0,0 +1,21 @@
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
float32 yaw_body # body angle in NED frame
float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame
bool R_valid # Set to true if rotation matrix is valid
# For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
float32[4] q_e # Attitude error in quaternion
bool q_e_valid # Set to true if quaternion error vector is valid
float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)

22
msg/vehicle_control_mode.msg Normal file
View File

@@ -0,0 +1,22 @@
uint64 timestamp # in microseconds since system start
# is set whenever the writing thread stores new data
bool flag_armed
bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing
# XXX needs yet to be set by state machine helper
bool flag_system_hil_enabled
bool flag_control_manual_enabled # true if manual input is mixed in
bool flag_control_auto_enabled # true if onboard autopilot should act
bool flag_control_offboard_enabled # true if offboard control should be used
bool flag_control_rates_enabled # true if rates are stabilized
bool flag_control_attitude_enabled # true if attitude stabilization is mixed in
bool flag_control_force_enabled # true if force control is mixed in
bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled
bool flag_control_position_enabled # true if position is controlled
bool flag_control_altitude_enabled # true if altitude is controlled
bool flag_control_climb_rate_enabled # true if climb rate is controlled
bool flag_control_termination_enabled # true if flighttermination is enabled

4
msg/vehicle_global_velocity_setpoint.msg Normal file
View File

@@ -0,0 +1,4 @@
# Velocity setpoint in NED frame
float32 vx # in m/s NED
float32 vy # in m/s NED
float32 vz # in m/s NED

36
msg/vehicle_local_position.msg Normal file
View File

@@ -0,0 +1,36 @@
# Fused local position in NED.
uint64 timestamp # Time of this estimate, in microseconds since system start
bool xy_valid # true if x and y are valid
bool z_valid # true if z is valid
bool v_xy_valid # true if vy and vy are valid
bool v_z_valid # true if vz is valid
# Position in local NED frame
float32 x # X position in meters in NED earth-fixed frame
float32 y # X position in meters in NED earth-fixed frame
float32 z # Z position in meters in NED earth-fixed frame (negative altitude)
# Velocity in NED frame
float32 vx # Ground X Speed (Latitude), m/s in NED
float32 vy # Ground Y Speed (Longitude), m/s in NED
float32 vz # Ground Z Speed (Altitude), m/s in NED
# Heading
float32 yaw
# Reference position in GPS / WGS84 frame
bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
bool z_global # true if z is valid and has valid global reference (ref_alt)
uint64 ref_timestamp # Time when reference position was set
float64 ref_lat # Reference point latitude in degrees
float64 ref_lon # Reference point longitude in degrees
float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
# Distance to surface
float32 dist_bottom # Distance to bottom surface (ground)
float32 dist_bottom_rate # Distance to bottom surface (ground) change rate
uint64 surface_bottom_timestamp # Time when new bottom surface found
bool dist_bottom_valid # true if distance to bottom surface is valid
float32 eph
float32 epv

7
msg/vehicle_local_position_setpoint.msg Normal file
View File

@@ -0,0 +1,7 @@
# Local position in NED frame
uint64 timestamp # timestamp of the setpoint
float32 x # in meters NED
float32 y # in meters NED
float32 z # in meters NED
float32 yaw # in radians NED -PI..+PI

6
msg/vehicle_rates_setpoint.msg Normal file
View File

@@ -0,0 +1,6 @@
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1

159
msg/vehicle_status.msg Normal file
View File

@@ -0,0 +1,159 @@
# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
uint8 MAIN_STATE_MANUAL = 0
uint8 MAIN_STATE_ALTCTL = 1
uint8 MAIN_STATE_POSCTL = 2
uint8 MAIN_STATE_AUTO_MISSION = 3
uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_MAX = 8
# If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well.
uint8 ARMING_STATE_INIT = 0
uint8 ARMING_STATE_STANDBY = 1
uint8 ARMING_STATE_ARMED = 2
uint8 ARMING_STATE_ARMED_ERROR = 3
uint8 ARMING_STATE_STANDBY_ERROR = 4
uint8 ARMING_STATE_REBOOT = 5
uint8 ARMING_STATE_IN_AIR_RESTORE = 6
uint8 ARMING_STATE_MAX = 7
uint8 HIL_STATE_OFF = 0
uint8 HIL_STATE_ON = 1
# Navigation state, i.e. "what should vehicle do".
uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6 # RC recover mode
uint8 NAVIGATION_STATE_AUTO_RTGS = 7 # Auto return to groundstation on data link loss
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down)
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_LAND = 11 # Land mode
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_MAX = 15
# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
uint8 VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64
uint8 VEHICLE_MODE_FLAG_HIL_ENABLED = 32
uint8 VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16
uint8 VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8
uint8 VEHICLE_MODE_FLAG_AUTO_ENABLED = 4
uint8 VEHICLE_MODE_FLAG_TEST_ENABLED = 2
uint8 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
# VEHICLE_TYPE, should match 1:1 MAVLink's MAV_TYPE ENUM
uint8 VEHICLE_TYPE_GENERIC = 0 # Generic micro air vehicle.
uint8 VEHICLE_TYPE_FIXED_WING = 1 # Fixed wing aircraft.
uint8 VEHICLE_TYPE_QUADROTOR = 2 # Quadrotor
uint8 VEHICLE_TYPE_COAXIAL = 3 # Coaxial helicopter
uint8 VEHICLE_TYPE_HELICOPTER = 4 # Normal helicopter with tail rotor.
uint8 VEHICLE_TYPE_ANTENNA_TRACKER = 5 # Ground installation
uint8 VEHICLE_TYPE_GCS = 6 # Operator control unit / ground control station
uint8 VEHICLE_TYPE_AIRSHIP = 7 # Airship, controlled
uint8 VEHICLE_TYPE_FREE_BALLOON = 8 # Free balloon, uncontrolled
uint8 VEHICLE_TYPE_ROCKET = 9 # Rocket
uint8 VEHICLE_TYPE_GROUND_ROVER = 10 # Ground rover
uint8 VEHICLE_TYPE_SURFACE_BOAT = 11 # Surface vessel, boat, ship
uint8 VEHICLE_TYPE_SUBMARINE = 12 # Submarine
uint8 VEHICLE_TYPE_HEXAROTOR = 13 # Hexarotor
uint8 VEHICLE_TYPE_OCTOROTOR = 14 # Octorotor
uint8 VEHICLE_TYPE_TRICOPTER = 15 # Octorotor
uint8 VEHICLE_TYPE_FLAPPING_WING = 16 # Flapping wing
uint8 VEHICLE_TYPE_KITE = 17 # Kite
uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18 # Onboard companion controller
uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19 # Vtol with two engines
uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines*/
uint8 VEHICLE_TYPE_ENUM_END = 21
uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage
# state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app.
uint16 counter # incremented by the writing thread everytime new data is stored
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state
uint8 hil_state # current hil state
bool failsafe # true if system is in failsafe state
int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum
int32 system_id # system id, inspired by MAVLink's system ID field
int32 component_id # subsystem / component id, inspired by MAVLink's component ID field
bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_returned_to_home
bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
bool condition_launch_position_valid # indicates a valid launch position
bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool condition_local_position_valid
bool condition_local_altitude_valid
bool condition_airspeed_valid # set to true by the commander app if there is a valid airspeed measurement available
bool condition_landed # true if vehicle is landed, always true if disarmed
bool condition_power_input_valid # set if input power is valid
float32 avionics_power_rail_voltage # voltage of the avionics power rail
bool rc_signal_found_once
bool rc_signal_lost # true if RC reception lost
uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost
bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
bool engine_failure # Set to true if an engine failure is detected
bool engine_failure_cmd # Set to true if an engine failure mode is commanded
bool gps_failure # Set to true if a gps failure is detected
bool gps_failure_cmd # Set to true if a gps failure mode is commanded
bool barometer_failure # Set to true if a barometer failure is detected
bool offboard_control_signal_found_once
bool offboard_control_signal_lost
bool offboard_control_signal_weak
uint64 offboard_control_signal_lost_interval # interval in microseconds without an offboard control message
bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC
# see SYS_STATUS mavlink message for the following
uint32 onboard_control_sensors_present
uint32 onboard_control_sensors_enabled
uint32 onboard_control_sensors_health
float32 load # processor load from 0 to 1
float32 battery_voltage
float32 battery_current
float32 battery_remaining
uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
uint16 drop_rate_comm
uint16 errors_comm
uint16 errors_count1
uint16 errors_count2
uint16 errors_count3
uint16 errors_count4
bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_engaged_gpsfailure_check

61
package.xml Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<package>
<name>px4</name>
<version>1.0.0</version>
<description>The PX4 Flight Stack package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="lorenz@px4.io">Lorenz Meier</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://px4.io/ros</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>message_generation</build_depend>
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<run_depend>message_runtime</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>eigen</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>eigen</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

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

@@ -55,6 +55,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -98,7 +102,7 @@ usage(const char *reason)
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
*
* The actual stack size should be set in the call
* to task_create().
*/
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/

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

@@ -571,7 +571,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
led_color_4 = LED_OFF;

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

@@ -75,6 +75,8 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
int
HIL::set_mode(Mode mode)
{
/*
/*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
/* multi-port as 4 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
@@ -514,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
// HIL always outputs at the alternate (usually faster) rate
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
// HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_SET(2):
@@ -660,7 +662,7 @@ int
hil_new_mode(PortMode new_mode)
{
// uint32_t gpio_bits;
// /* reset to all-inputs */
// g_hil->ioctl(0, GPIO_RESET, 0);
@@ -692,17 +694,17 @@ hil_new_mode(PortMode new_mode)
/* select 2-pin PWM mode */
servo_mode = HIL::MODE_2PWM;
break;
case PORT2_8PWM:
/* select 8-pin PWM mode */
servo_mode = HIL::MODE_8PWM;
break;
case PORT2_12PWM:
/* select 12-pin PWM mode */
servo_mode = HIL::MODE_12PWM;
break;
case PORT2_16PWM:
/* select 16-pin PWM mode */
servo_mode = HIL::MODE_16PWM;

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

@@ -74,6 +74,7 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>

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

@@ -70,6 +70,10 @@
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -138,11 +142,11 @@ private:
uint32_t _groups_required;
uint32_t _groups_subscribed;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int _actuator_output_topic_instance;
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
pwm_limit_t _pwm_limit;
@@ -508,7 +512,7 @@ PX4FMU::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -585,7 +589,7 @@ PX4FMU::task_main()
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
@@ -612,7 +616,7 @@ PX4FMU::task_main()
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -745,7 +749,7 @@ PX4FMU::task_main()
}
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
@@ -1142,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
* FMUv1
*/
switch (arg) {
case 0:

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

@@ -75,6 +75,10 @@
#include <systemlib/circuit_breaker.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
@@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state()
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
@@ -2585,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_RC_CONFIG: {
/* enable setting of RC configuration without relying
on param_get()
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= RC_INPUT_MAX_CHANNELS) {

17
src/drivers/roboclaw/RoboClaw.cpp
View File

@@ -54,6 +54,7 @@
#include <mavlink/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>
@@ -65,7 +66,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
_pulsesPerRev(pulsesPerRev),
_uart(0),
_controlPoll(),
_actuators(NULL, ORB_ID(actuator_controls_0), 20),
_actuators(ORB_ID(actuator_controls_0), 20),
_motor1Position(0),
_motor1Speed(0),
_motor1Overflow(0),
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
uint8_t rbuf[50];
usleep(5000);
int nread = read(_uart, rbuf, 50);
if (nread < 6) {
if (nread < 6) {
printf("failed to read\n");
return -1;
}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5];
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask;
// check if checksum is valid
if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) {
_motor1Overflow += overFlow;
_motor1Position = float(int64_t(count) +
_motor1Position = float(int64_t(count) +
_motor1Overflow*overflowAmount)/_pulsesPerRev;
} else if (motor == MOTOR_2) {
_motor2Overflow += overFlow;
_motor2Position = float(int64_t(count) +
_motor2Position = float(int64_t(count) +
_motor2Overflow*overflowAmount)/_pulsesPerRev;
}
return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
return -1;
}
int RoboClaw::resetEncoders()
int RoboClaw::resetEncoders()
{
uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
return sum;
}
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
size_t n_data, uint16_t & prev_sum)
{
tcflush(_uart, TCIOFLUSH); // flush buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
return write(_uart, buf, n_data + 3);
}
int roboclawTest(const char *deviceName, uint8_t address,
int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev)
{
printf("roboclaw test: starting\n");

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

@@ -61,6 +61,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
@@ -108,7 +112,7 @@ static void usage(const char *reason);
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
@@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
/*
/*
* The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector
* is structured as:
*
* Control Group 0 (attitude):
*
*
* 0 - roll (-1..+1)
* 1 - pitch (-1..+1)
* 2 - yaw (-1..+1)
@@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
*
* Wikipedia description:
* http://en.wikipedia.org/wiki/Publish–subscribe_pattern
*
*
*/
@@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/*
* Declare and safely initialize all structs to zero.
*
*
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
@@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (ret < 0) {
/*
* Poll error, this will not really happen in practice,
* but its good design practice to make output an error message.
* but its good design practice to make output an error message.
*/
warnx("poll error");
@@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
if (manual_sp_updated)
/* get the RC (or otherwise user based) input */
/* get the RC (or otherwise user based) input */
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */

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

@@ -67,6 +67,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <poll.h>
#include <platforms/px4_defines.h>
#include "flow_position_estimator_params.h"
@@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
{
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
sum = sum + speed[j] * att.R[i][j];
sum = sum + speed[j] * PX4_R(att.R, i, j);
global_speed[i] = sum;
}

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

@@ -47,6 +47,10 @@
#include <systemlib/err.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/uORB.h>
__EXPORT int ex_hwtest_main(int argc, char *argv[]);

44
src/examples/publisher/module.mk Normal file
View File

@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Publisher Example Application
#
MODULE_COMMAND = publisher
SRCS = publisher_main.cpp \
publisher_start_nuttx.cpp \
publisher_example.cpp
MODULE_STACKSIZE = 1200

83
src/examples/publisher/publisher_example.cpp Normal file
View File

@@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file publisher_example.cpp
* Example subscriber for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "publisher_example.h"
using namespace px4;
PublisherExample::PublisherExample() :
_n(),
_rc_channels_pub(_n.advertise<px4_rc_channels>()),
_v_att_pub(_n.advertise<px4_vehicle_attitude>()),
_parameter_update_pub(_n.advertise<px4_parameter_update>())
{
}
int PublisherExample::main()
{
px4::Rate loop_rate(10);
while (px4::ok()) {
loop_rate.sleep();
_n.spinOnce();
/* Publish example message */
px4_rc_channels rc_channels_msg;
rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros();
PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid);
_rc_channels_pub->publish(rc_channels_msg);
/* Publish example message */
px4_vehicle_attitude v_att_msg;
v_att_msg.data().timestamp = px4::get_time_micros();
PX4_INFO("att: %llu", v_att_msg.data().timestamp);
_v_att_pub->publish(v_att_msg);
/* Publish example message */
px4_parameter_update parameter_update_msg;
parameter_update_msg.data().timestamp = px4::get_time_micros();
PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp);
_parameter_update_pub->publish(parameter_update_msg);
}
return 0;
}

55
src/examples/publisher/publisher_example.h Normal file
View File

@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file publisher.h
* Example publisher for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <px4.h>
class PublisherExample {
public:
PublisherExample();
~PublisherExample() {};
int main();
protected:
px4::NodeHandle _n;
px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub;
px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub;
};

56
src/examples/publisher/publisher_main.cpp Normal file
View File

@@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file publisher_main.cpp
* Example publisher for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "publisher_example.h"
bool thread_running = false; /**< Deamon status flag */
int main(int argc, char **argv)
{
px4::init(argc, argv, "publisher");
PX4_INFO("starting");
PublisherExample p;
thread_running = true;
p.main();
PX4_INFO("exiting.");
thread_running = false;
return 0;
}

99
src/examples/publisher/publisher_start_nuttx.cpp Normal file
View File

@@ -0,0 +1,99 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file publisher_start_nuttx.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <cstdlib>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
}
using namespace px4;
extern int main(int argc, char **argv);
extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
int publisher_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "usage: publisher {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
task_should_exit = false;
daemon_task = task_spawn_cmd("publisher",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
warnx("unrecognized command");
return 1;
}

45
src/examples/subscriber/module.mk Normal file
View File

@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Subscriber Example Application
#
MODULE_COMMAND = subscriber
SRCS = subscriber_main.cpp \
subscriber_start_nuttx.cpp \
subscriber_example.cpp \
subscriber_params.c
MODULE_STACKSIZE = 2400

105
src/examples/subscriber/subscriber_example.cpp Normal file
View File

@@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subscriber_example.cpp
* Example subscriber for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "subscriber_params.h"
#include "subscriber_example.h"
using namespace px4;
void rc_channels_callback_function(const px4_rc_channels &msg) {
PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
}
SubscriberExample::SubscriberExample() :
_n(),
_p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
_interval(0),
_p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
_test_float(0.0f)
{
/* Read the parameter back as example */
PX4_PARAM_GET(_p_sub_interv, &_interval);
PX4_INFO("Param SUB_INTERV = %d", _interval);
PX4_PARAM_GET(_p_test_float, &_test_float);
PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
/* Do some subscriptions */
/* Function */
_n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval);
/* No callback */
_sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
/* Class method */
_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
/* Another class method */
_n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000);
/* Yet antoher class method */
_n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000);
PX4_INFO("subscribed");
}
/**
* This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
* Also the current value of the _sub_rc_chan subscription is printed
*/
void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
PX4_INFO("rc_channels_callback (method): [%llu]",
msg.data().timestamp_last_valid);
PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
_sub_rc_chan->data().timestamp_last_valid);
}
void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
PX4_INFO("vehicle_attitude_callback (method): [%llu]",
msg.data().timestamp);
}
void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) {
PX4_INFO("parameter_update_callback (method): [%llu]",
msg.data().timestamp);
PX4_PARAM_GET(_p_sub_interv, &_interval);
PX4_INFO("Param SUB_INTERV = %d", _interval);
PX4_PARAM_GET(_p_test_float, &_test_float);
PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
}

65
src/examples/subscriber/subscriber_example.h Normal file
View File

@@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subscriber_example.h
* Example subscriber for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4.h>
using namespace px4;
void rc_channels_callback_function(const px4_rc_channels &msg);
class SubscriberExample {
public:
SubscriberExample();
~SubscriberExample() {};
void spin() {_n.spin();}
protected:
px4::NodeHandle _n;
px4_param_t _p_sub_interv;
int32_t _interval;
px4_param_t _p_test_float;
float _test_float;
px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
void rc_channels_callback(const px4_rc_channels &msg);
void vehicle_attitude_callback(const px4_vehicle_attitude &msg);
void parameter_update_callback(const px4_parameter_update &msg);
};

55
src/examples/subscriber/subscriber_main.cpp Normal file
View File

@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subscriber_main.cpp
* Example subscriber for ros and px4
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "subscriber_example.h"
bool thread_running = false; /**< Deamon status flag */
int main(int argc, char **argv)
{
px4::init(argc, argv, "subscriber");
PX4_INFO("starting");
SubscriberExample s;
thread_running = true;
s.spin();
PX4_INFO("exiting.");
thread_running = false;
return 0;
}

37
src/modules/uORB/topics/vehicle_global_velocity_setpoint.h → src/examples/subscriber/subscriber_params.c
View File

@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,31 +32,25 @@
****************************************************************************/
/**
* @file vehicle_global_velocity_setpoint.h
* Definition of the global velocity setpoint uORB topic.
* @file subscriber_params.c
* Parameters for the subscriber example
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
#include "../uORB.h"
#include <px4_defines.h>
#include "subscriber_params.h"
/**
* @addtogroup topics
* @{
* Interval of one subscriber in the example in ms
*
* @group Subscriber Example
*/
struct vehicle_global_velocity_setpoint_s {
float vx; /**< in m/s NED */
float vy; /**< in m/s NED */
float vz; /**< in m/s NED */
}; /**< Velocity setpoint in NED frame */
PX4_PARAM_DEFINE_INT32(SUB_INTERV);
/**
* @}
* Float Demonstration Parameter in the Example
*
* @group Subscriber Example
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_global_velocity_setpoint);
#endif
PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);

43
src/examples/subscriber/subscriber_params.h Normal file
View File

@@ -0,0 +1,43 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subscriber_params.h
* Default parameters for the subscriber example
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#define PARAM_SUB_INTERV_DEFAULT 100
#define PARAM_SUB_TESTF_DEFAULT 3.14f

99
src/examples/subscriber/subscriber_start_nuttx.cpp Normal file
View File

@@ -0,0 +1,99 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subscriber_start_nuttx.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <cstdlib>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
}
using namespace px4;
extern int main(int argc, char **argv);
extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
int subscriber_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "usage: subscriber {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
task_should_exit = false;
daemon_task = task_spawn_cmd("subscriber",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
warnx("unrecognized command");
return 1;
}

48
src/include/px4.h Normal file
View File

@@ -0,0 +1,48 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4.h
*
* Main system header with common convenience functions
*/
#pragma once
#include "../platforms/px4_includes.h"
#include "../platforms/px4_defines.h"
#ifdef __cplusplus
#include "../platforms/px4_middleware.h"
#include "../platforms/px4_nodehandle.h"
#include "../platforms/px4_subscriber.h"
#endif

5
src/lib/geo/geo.h
View File

@@ -44,10 +44,7 @@
*/
#pragma once
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
#include <platforms/px4_defines.h>
__BEGIN_DECLS
#include "geo_lookup/geo_mag_declination.h"

5
src/lib/mathlib/math/Limits.cpp
View File

@@ -46,6 +46,9 @@
namespace math {
#ifndef CONFIG_ARCH_ARM
#define M_PI_F 3.14159265358979323846f
#endif
float __EXPORT min(float val1, float val2)
{
@@ -143,4 +146,4 @@ double __EXPORT degrees(double radians)
return (radians / M_PI) * 180.0;
}
}
}

4
src/lib/mathlib/math/Limits.hpp
View File

@@ -39,7 +39,7 @@
#pragma once
#include <nuttx/config.h>
#include <platforms/px4_defines.h>
#include <stdint.h>
namespace math {
@@ -84,4 +84,4 @@ float __EXPORT degrees(float radians);
double __EXPORT degrees(double radians);
}
}

58
src/lib/mathlib/math/Matrix.hpp
View File

@@ -44,12 +44,20 @@
#define MATRIX_HPP
#include <stdio.h>
#include <math.h>
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include <platforms/ros/eigen_math.h>
#include <Eigen/Eigen>
#endif
#include <platforms/px4_defines.h>
namespace math
{
template <unsigned int M, unsigned int N>
template<unsigned int M, unsigned int N>
class __EXPORT Matrix;
// MxN matrix with float elements
@@ -65,7 +73,11 @@ public:
/**
* struct for using arm_math functions
*/
#ifdef CONFIG_ARCH_ARM
arm_matrix_instance_f32 arm_mat;
#else
eigen_matrix_instance arm_mat;
#endif
/**
* trivial ctor
@@ -114,6 +126,15 @@ public:
memcpy(data, d, sizeof(data));
}
#if defined(__PX4_ROS)
/**
* set data from boost::array
*/
void set(const boost::array<float, 9ul> d) {
set(static_cast<const float*>(d.data()));
}
#endif
/**
* access by index
*/
@@ -273,27 +294,53 @@ public:
*/
template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
#ifdef CONFIG_ARCH_ARM
Matrix<M, P> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
#else
Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
(this->arm_mat.pData);
Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
(m.arm_mat.pData);
Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
Matrix<M, P> res(Product.data());
return res;
#endif
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
(this->arm_mat.pData);
Me.transposeInPlace();
Matrix<N, M> res(Me.data());
return res;
#endif
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
#ifdef CONFIG_ARCH_ARM
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
(this->arm_mat.pData);
Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
Matrix<M, N> res(MyInverse.data());
return res;
#endif
}
/**
@@ -352,8 +399,17 @@ public:
* multiplication by a vector
*/
Vector<M> operator *(const Vector<N> &v) const {
#ifdef CONFIG_ARCH_ARM
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
#else
//probably nicer if this could go into a function like "eigen_mat_mult" or so
Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
(this->arm_mat.pData);
Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
Eigen::VectorXf Product = Me * Vec;
Vector<M> res(Product.data());
#endif
return res;
}
};

2
src/lib/mathlib/math/Quaternion.hpp
View File

@@ -44,7 +44,7 @@
#define QUATERNION_HPP
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
#include "Vector.hpp"
#include "Matrix.hpp"

12
src/lib/mathlib/math/Vector.hpp
View File

@@ -45,7 +45,14 @@
#include <stdio.h>
#include <math.h>
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include <platforms/ros/eigen_math.h>
#endif
#include <platforms/px4_defines.h>
namespace math
{
@@ -65,7 +72,12 @@ public:
/**
* struct for using arm_math functions, represents column vector
*/
#ifdef CONFIG_ARCH_ARM
arm_matrix_instance_f32 arm_col;
#else
eigen_matrix_instance arm_col;
#endif
/**
* trivial ctor

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

@@ -592,7 +592,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
R = R_decl * R_body;
/* copy rotation matrix */
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {

5
src/modules/bottle_drop/bottle_drop.cpp
View File

@@ -57,8 +57,13 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>

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

@@ -74,6 +74,10 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
@@ -364,31 +368,31 @@ void print_status()
const char *armed_str;
switch (state.arming_state) {
case ARMING_STATE_INIT:
case vehicle_status_s::ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
case vehicle_status_s::ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
case vehicle_status_s::ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
case vehicle_status_s::ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
case vehicle_status_s::ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
case vehicle_status_s::ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
@@ -411,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
@@ -448,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state
@@ -458,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
}
}
}
@@ -521,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -546,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
unsigned int mav_goto = (cmd->param1 + 0.5f);
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -664,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -794,41 +798,41 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
const char *main_states_str[MAIN_STATE_MAX];
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[ARMING_STATE_INIT] = "INIT";
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
const char *nav_states_str[NAVIGATION_STATE_MAX];
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -849,10 +853,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.nav_state = NAVIGATION_STATE_MANUAL;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
@@ -865,7 +869,7 @@ int commander_thread_main(int argc, char *argv[])
status.data_link_lost = true;
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
// XXX for now just set sensors as initialized
@@ -1139,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[])
}
/* disable manual override for all systems that rely on electronic stabilization */
if (status.system_type == VEHICLE_TYPE_COAXIAL ||
status.system_type == VEHICLE_TYPE_HELICOPTER ||
status.system_type == VEHICLE_TYPE_TRICOPTER ||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
@@ -1155,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
@@ -1306,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(safety), safety_sub, &safety);
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
@@ -1340,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
@@ -1512,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
@@ -1520,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[])
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1531,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1545,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1658,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -1684,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1692,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if (status.main_state != MAIN_STATE_MANUAL) {
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1715,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
}
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
} else {
@@ -1853,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state != MAIN_STATE_MANUAL &&
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1877,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state == MAIN_STATE_ACRO ||
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1972,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[])
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
@@ -2068,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
bool set_normal_color = false;
/* set mode */
if (status_local->arming_state == ARMING_STATE_ARMED) {
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2087,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) {
/* set color */
if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
} else {
if (status_local->condition_local_position_valid) {
@@ -2145,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* if offboard is set allready by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
}
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -2162,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_ACRO);
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else {
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2189,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->posctl_switch != SWITCH_POS_ON) {
if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
print_reject_mode(status_local, "ALTCTL");
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2215,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2231,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_LOITER");
} else {
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2240,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2248,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@@ -2279,11 +2283,11 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {
case NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
@@ -2295,7 +2299,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2307,7 +2311,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2319,12 +2323,12 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RCRECOVER:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2336,7 +2340,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2348,7 +2352,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2361,7 +2365,7 @@ set_control_mode()
break;
case NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2374,7 +2378,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
@@ -2387,7 +2391,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@@ -2400,7 +2404,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = true;
break;
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
@@ -2599,7 +2603,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
@@ -2663,7 +2667,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}

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

@@ -72,16 +72,16 @@ static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
}
static int buzzer = -1;

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

@@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transtion, the requested state to transition to and finally the expected
// machine state after transition.
@@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
@@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby error",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to reboot",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to init",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to standby error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to reboot",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to standby",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to armed error",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed error to standby error",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby error to reboot",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to armed",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to reboot",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// Safety switch arming tests
{ "transition: standby to armed, no safety switch",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to armed, safety switch off",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// standby error
{ "transition: armed error to standby error requested standby",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby to armed error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to init",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to reboot",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to armed",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to reboot",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to armed",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to standby",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: reboot to armed",
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: in air restore to standby",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Sensor tests
{ "no transition: init to standby - sensors not initialized",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{ "no transition: init to standby, safety switch on",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status;
struct safety_s safety;
struct actuator_armed_s armed;
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
@@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
main_state_t to_state; // State to transition to
transition_result_t expected_transition_result; // Expected result from main_state_transition call
} MainTransitionTest_t;
// Bits for condition_bits
#define MTT_ALL_NOT_VALID 0
#define MTT_ROTARY_WING 1 << 0
@@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
#define MTT_LOC_POS_VALID 1 << 2
#define MTT_HOME_POS_VALID 1 << 3
#define MTT_GLOBAL_POS_VALID 1 << 4
static const MainTransitionTest_t rgMainTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
{ "transition: MANUAL to ACRO",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
{ "transition: ACRO to MANUAL",
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
{ "transition: AUTO_LOITER to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - not rotary",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: ALTCTL to MANUAL - local altitude valid",
MTT_LOC_ALT_VALID,
MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
MTT_LOC_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: POSCTL to MANUAL - local position valid, global position valid",
MTT_LOC_POS_VALID,
MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
MTT_ROTARY_WING,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
};
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
for (size_t i=0; i<cMainTransitionTests; i++) {
const MainTransitionTest_t* test = &rgMainTransitionTests[i];
@@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
// Attempt transition
transition_result_t result = main_state_transition(&current_state, test->to_state);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
if (test->expected_transition_result == result) {
@@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void)
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
return (_tests_failed == 0);
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

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

@@ -76,19 +76,19 @@ static const int ERROR = -1;
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char * const state_names[ARMING_STATE_MAX] = {
static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
@@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
} else {
@@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == HIL_STATE_OFF) {
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
@@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
@@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
@@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
case MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
@@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_POSCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
@@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_OFFBOARD:
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
@@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_MAX:
case vehicle_status_s::MAIN_STATE_MAX:
default:
break;
}
@@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
} else {
switch (new_state) {
case HIL_STATE_OFF:
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
/* Disable publication of all attached sensors */
/* list directory */
@@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
case vehicle_status_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
case vehicle_status_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
case vehicle_status_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
@@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
@@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
@@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
@@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
case MAIN_STATE_OFFBOARD:
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = NAVIGATION_STATE_POSCTL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_OFFBOARD;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;

4
src/modules/controllib/block/Block.cpp
View File

@@ -101,7 +101,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
void Block::updatePublications()
{
uORB::PublicationBase *pub = getPublications().getHead();
uORB::PublicationNode *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {

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