diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 0000000000..52fd97e7ea --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/.gitmodules b/.gitmodules index 4b84afac2a..4996b274bd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,9 @@ [submodule "uavcan"] path = 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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b3ca17e31..489467db7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -111,7 +112,9 @@ target_link_libraries(px4 ) ## Declare a test publisher -add_executable(publisher src/examples/publisher/publisher.cpp) +add_executable(publisher + src/examples/publisher/publisher_main.cpp + src/examples/publisher/publisher_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -124,7 +127,9 @@ target_link_libraries(publisher ) ## Declare a test subscriber -add_executable(subscriber src/examples/subscriber/subscriber.cpp) +add_executable(subscriber + src/examples/subscriber/subscriber_main.cpp + src/examples/subscriber/subscriber_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -136,13 +141,15 @@ target_link_libraries(subscriber px4 ) -## mc_att_control -add_executable(mc_att_control src/module/mc_att_control_main.cpp) -add_dependencies(mc_att_control px4_generate_messages_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) +# add_executable(mc_att_control + # src/modules/mc_att_control/mc_att_control_main.cpp + # src/moudles/mc_att_control/mc_att_control_base.cpp) +# add_dependencies(mc_att_control px4_generate_messages_cpp) +# target_link_libraries(mc_att_control + # ${catkin_LIBRARIES} + # px4 +# ) + ############# ## Install ## diff --git a/Makefile b/Makefile index 7df8004a40..f2e467e5aa 100644 --- a/Makefile +++ b/Makefile @@ -227,12 +227,18 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +TOPICS_TEMPORARY_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) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ - -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR)) + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) +# clean up temporary files + $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) # # Testing targets diff --git a/NuttX b/NuttX index ae4b05e2c5..89a6776fc1 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c +Subproject commit 89a6776fc1b31d242c637d3f19072609bb98ec6c diff --git a/Tools/gencpp b/Tools/gencpp new file mode 160000 index 0000000000..26a86f04bc --- /dev/null +++ b/Tools/gencpp @@ -0,0 +1 @@ +Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a diff --git a/Tools/genmsg b/Tools/genmsg new file mode 160000 index 0000000000..72f0383f0e --- /dev/null +++ b/Tools/genmsg @@ -0,0 +1 @@ +Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454 diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index a738dcb7ef..2ddbd69847 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -39,6 +39,8 @@ message files """ from __future__ import print_function import os +import shutil +import filecmp import argparse import genmsg.template_tools @@ -55,44 +57,93 @@ package = 'px4' def convert_file(filename, outputdir, templatedir, includepath): - """ - Converts a single .msg file to a uorb header - """ - print("Generating uORB headers from {0}".format(filename)) - genmsg.template_tools.generate_from_file(filename, - package, - outputdir, - templatedir, - includepath, - msg_template_map, - srv_template_map) + """ + Converts a single .msg file to a uorb header + """ + print("Generating uORB 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) + """ + 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): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + 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, 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): + """ + 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) 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') - args = parser.parse_args() + 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') + 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(args.dir, args.outputdir, args.templatedir) + 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) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index df6b6d0c5b..38c5ebc7b3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -113,6 +113,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # OBC challenge diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a0876..435c240bfa 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -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 diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a2..b98b1a0afe 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -121,7 +121,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 # diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg new file mode 100644 index 0000000000..b83adb8f24 --- /dev/null +++ b/msg/px4_msgs/actuator_armed.msg @@ -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 diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/px4_msgs/actuator_controls.msg new file mode 100644 index 0000000000..743f20cdff --- /dev/null +++ b/msg/px4_msgs/actuator_controls.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/px4_msgs/manual_control_setpoint.msg new file mode 100644 index 0000000000..d3cfb078be --- /dev/null +++ b/msg/px4_msgs/manual_control_setpoint.msg @@ -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 diff --git a/msg/px4_msgs/parameter_update.msg b/msg/px4_msgs/parameter_update.msg new file mode 100644 index 0000000000..39dc336e05 --- /dev/null +++ b/msg/px4_msgs/parameter_update.msg @@ -0,0 +1 @@ +uint64 timestamp # time at which the latest parameter was updated diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg deleted file mode 100644 index f6bf583073..0000000000 --- a/msg/px4_msgs/unused/actuator_armed.msg +++ /dev/null @@ -1,6 +0,0 @@ - -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 diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg new file mode 100644 index 0000000000..1a8e6e3d54 --- /dev/null +++ b/msg/px4_msgs/vehicle_attitude_setpoint.msg @@ -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) diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/px4_msgs/vehicle_control_mode.msg new file mode 100644 index 0000000000..153a642bb4 --- /dev/null +++ b/msg/px4_msgs/vehicle_control_mode.msg @@ -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 diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/px4_msgs/vehicle_rates_setpoint.msg new file mode 100644 index 0000000000..834113c798 --- /dev/null +++ b/msg/px4_msgs/vehicle_rates_setpoint.msg @@ -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 diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 8cc9ed6862..5a23932d3a 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include "flow_position_estimator_params.h" diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 0fc4316ecb..4f941cd439 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = publisher -SRCS = publisher.cpp +SRCS = publisher_main.cpp \ + publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp deleted file mode 100644 index 8ef147493f..0000000000 --- a/src/examples/publisher/publisher.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple sending of messages over the PX4 middleware system. - */ - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(publisher) -{ - - px4::init(argc, argv, "px4_publisher"); - - px4::NodeHandle n; - - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels); - - - px4::Rate loop_rate(10); - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - - while (px4::ok()) { - /** - * This is a message object. You stuff it with data, and then publish it. - */ - PX4_TOPIC_T(rc_channels) msg; - - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - rc_channels_pub->publish(msg); - - n.spinOnce(); - - loop_rate.sleep(); - ++count; - } - - return 0; -} diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp new file mode 100644 index 0000000000..2e5779ebec --- /dev/null +++ b/src/examples/publisher/publisher_example.cpp @@ -0,0 +1,69 @@ + +/**************************************************************************** + * + * 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 + */ + +#include "publisher_example.h" + +using namespace px4; + +PublisherExample::PublisherExample() : + _n(), + _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) +{ + +} + +int PublisherExample::main() +{ + px4::Rate loop_rate(10); + + while (px4::ok()) { + PX4_TOPIC_T(rc_channels) msg; + msg.timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.timestamp_last_valid); + + _rc_channels_pub->publish(msg); + + _n.spinOnce(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/src/modules/uORB/topics/parameter_update.h b/src/examples/publisher/publisher_example.h similarity index 79% rename from src/modules/uORB/topics/parameter_update.h rename to src/examples/publisher/publisher_example.h index 68964deb0a..304ecef47a 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/examples/publisher/publisher_example.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +32,21 @@ ****************************************************************************/ /** - * @file parameter_update.h - * Notification about a parameter update. + * @file publisher.h + * Example publisher for ros and px4 + * + * @author Thomas Gubler */ +#include -#ifndef TOPIC_PARAMETER_UPDATE_H -#define TOPIC_PARAMETER_UPDATE_H +class PublisherExample { +public: + PublisherExample(); -#include -#include "../uORB.h" + ~PublisherExample() {}; -/** - * @addtogroup topics - * @{ - */ - -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; + int main(); +protected: + px4::NodeHandle _n; + px4::Publisher * _rc_channels_pub; }; - -/** - * @} - */ - -ORB_DECLARE(parameter_update); - -#endif \ No newline at end of file diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp new file mode 100644 index 0000000000..5b50ecc6ca --- /dev/null +++ b/src/examples/publisher/publisher_main.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 + */ +#include +#include +#include "publisher_example.h" + +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +PX4_MAIN_FUNCTION(publisher); + +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +extern "C" __EXPORT 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, + publisher_task_main, + (argv) ? (const char **)&argv[2] : (const char **)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; +} +#endif + +PX4_MAIN_FUNCTION(publisher) +{ + px4::init(argc, argv, "publisher"); + + PX4_INFO("starting"); + PublisherExample p; + thread_running = true; + p.main(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 481c36ea70..3156ebb30a 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,6 +37,8 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp +SRCS = subscriber_main.cpp \ + subscriber_example.cpp \ + subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp deleted file mode 100644 index a29f9ab934..0000000000 --- a/src/examples/subscriber/subscriber.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. - */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); -} -void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid); -} - -class RCHandler { -public: - void callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); - } -}; - -RCHandler rchandler; - - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(subscriber) { - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of px4::init() before using any other - * part of the PX4/ ROS system. - */ - px4::init(argc, argv, "listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - px4::NodeHandle n; - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); - PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); - PX4_INFO("subscribed"); - - /** - * px4::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). px4::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - n.spin(); - PX4_INFO("finished, returning"); - - return 0; -} diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp new file mode 100644 index 0000000000..83ead4ba5a --- /dev/null +++ b/src/examples/subscriber/subscriber_example.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 + */ + +#include "subscriber_params.h" +#include "subscriber_example.h" + +using namespace px4; + +void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard: [%llu]", msg.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 */ + PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); + /* Class Method */ + PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + /* No callback */ + _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + + 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_TOPIC_T(rc_channels) &msg) { + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + _sub_rc_chan->get_msg().timestamp_last_valid); +} diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/examples/subscriber/subscriber_example.h similarity index 67% rename from src/modules/uORB/topics/actuator_armed.h rename to src/examples/subscriber/subscriber_example.h index 0f6c9aca17..c4b853d4de 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/examples/subscriber/subscriber_example.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,38 +32,31 @@ ****************************************************************************/ /** - * @file actuator_armed.h - * - * Actuator armed topic + * @file subscriber_example.h + * Example subscriber for ros and px4 * + * @author Thomas Gubler */ +#include -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H +using namespace px4; -#include -#include "../uORB.h" +class SubscriberExample { +public: + SubscriberExample(); -/** - * @addtogroup topics - * @{ - */ + ~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::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + + void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - uint64_t 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 */ }; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); - -#endif diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp new file mode 100644 index 0000000000..716233739b --- /dev/null +++ b/src/examples/subscriber/subscriber_main.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 + */ +#include +#include +#include "subscriber_example.h" + +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +PX4_MAIN_FUNCTION(subscriber); + +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +extern "C" __EXPORT 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, + subscriber_task_main, + (argv) ? (const char **)&argv[2] : (const char **)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; +} +#endif + +PX4_MAIN_FUNCTION(subscriber) +{ + px4::init(argc, argv, "subscriber"); + + PX4_INFO("starting"); + SubscriberExample s; + thread_running = true; + s.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/examples/subscriber/subscriber_params.c similarity index 67% rename from src/modules/uORB/topics/vehicle_rates_setpoint.h rename to src/examples/subscriber/subscriber_params.c index 9f8b412a7d..a43817b5b9 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/examples/subscriber/subscriber_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * 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,35 +32,25 @@ ****************************************************************************/ /** - * @file vehicle_rates_setpoint.h - * Definition of the vehicle rates setpoint topic + * @file subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler */ -#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ -#define TOPIC_VEHICLE_RATES_SETPOINT_H_ - -#include -#include "../uORB.h" +#include +#include "subscriber_params.h" /** - * @addtogroup topics - * @{ + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example */ -struct vehicle_rates_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start */ - - float roll; /**< body angular rates in NED frame */ - float pitch; /**< body angular rates in NED frame */ - float yaw; /**< body angular rates in NED frame */ - float thrust; /**< thrust normalized to 0..1 */ - -}; /**< vehicle_rates_setpoint */ +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** -* @} -*/ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_rates_setpoint); - -#endif + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 0000000000..f6f1d6ba0c --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -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 + */ +#pragma once + +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/include/px4.h b/src/include/px4.h index 45068a6f77..ca702d63ce 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -39,27 +39,8 @@ #pragma once -#include - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#include "ros/ros.h" -#include "px4/rc_channels.h" - -#else -/* - * Building for NuttX - */ -#include -#include -#include -#include - -#endif - -#include +#include "../platforms/px4_includes.h" +#include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" diff --git a/src/include/px4_defines.h b/src/include/px4_defines.h deleted file mode 100644 index 2dd57940d1..0000000000 --- a/src/include/px4_defines.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * 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_defines.h - * - * Generally used magic defines - */ - -#pragma once - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#define __EXPORT -#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO -#define PX4_TOPIC(_name) #_name -#define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -#else -/* - * Building for NuttX - */ -#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx -#define PX4_WARN warnx -#define PX4_INFO warnx -#define PX4_TOPIC(_name) ORB_ID(_name) -#define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -#endif - -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ -#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) -#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) - -/* wrapper for 2d matrices */ -#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) - -/* wrapper for rotation matrices stored in arrays */ -#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a0..e16f33bd68 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -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; } -} \ No newline at end of file +} diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd669..fca4197b8d 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,7 @@ #pragma once -#include +#include #include namespace math { @@ -84,4 +84,4 @@ float __EXPORT degrees(float radians); double __EXPORT degrees(double radians); -} \ No newline at end of file +} diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ca931e2daf..806f5933ad 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,12 +44,20 @@ #define MATRIX_HPP #include +#include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#include +#define M_PI_2_F 1.5707963267948966192f +#endif namespace math { -template +template class __EXPORT Matrix; // MxN matrix with float elements @@ -65,16 +73,19 @@ 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 * Initializes the elements to zero. */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} - { + MatrixBase() : + data {}, + arm_mat {M, N, &data[0][0]} { } virtual ~MatrixBase() {}; @@ -83,20 +94,17 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat{M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} - { + MatrixBase(const float d[M][N]) : + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -148,8 +156,9 @@ public: bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) + if (data[i][j] != m.data[i][j]) { return false; + } return true; } @@ -160,8 +169,9 @@ public: bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) + if (data[i][j] != m.data[i][j]) { return true; + } return false; } @@ -181,8 +191,9 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { res.data[i][j] = -data[i][j]; + } return res; } @@ -194,16 +205,18 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { res.data[i][j] = data[i][j] + m.data[i][j]; + } return res; } Matrix &operator +=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { data[i][j] += m.data[i][j]; + } return *static_cast*>(this); } @@ -215,16 +228,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res.data[i][j] = data[i][j] - m.data[i][j]; + } return res; } Matrix &operator -=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { data[i][j] -= m.data[i][j]; + } return *static_cast*>(this); } @@ -236,16 +251,19 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res.data[i][j] = data[i][j] * num; + } return res; + } Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { data[i][j] *= num; + } return *static_cast*>(this); } @@ -254,16 +272,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res[i][j] = data[i][j] / num; + } return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { data[i][j] /= num; + } return *static_cast*>(this); } @@ -273,27 +293,53 @@ public: */ template Matrix operator *(const Matrix &m) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map > + (m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; + Matrix res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Me.transposeInPlace(); + Matrix res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + Matrix res(MyInverse.data()); + return res; +#endif } /** @@ -310,16 +356,18 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) + for (unsigned int i = 0; i < n; i++) { data[i][i] = 1; + } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { printf("%.3f\t", data[i][j]); + } printf(" ]\n"); } @@ -352,8 +400,17 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { +#ifdef CONFIG_ARCH_ARM Vector 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 Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); + Eigen::VectorXf Product = Me * Vec; + Vector res(Product.data()); +#endif return res; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 21d05c7ef1..b3cca30c6c 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,7 +44,7 @@ #define QUATERNION_HPP #include -#include "../CMSIS/Include/arm_math.h" + #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 0ddf776155..57b45e3ab2 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -45,7 +45,12 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#endif namespace math { @@ -65,7 +70,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 diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 0000000000..fea1a773e3 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -0,0 +1,298 @@ +function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + + +%LQG Postion Estimator and Controller +% Observer: +% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) +% x[n+1|n] = Ax[n|n] + Bu[n] +% +% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ +% +% +% Arguments: +% approx_prediction: if 1 then the exponential map is approximated with a +% first order taylor approximation. has at the moment not a big influence +% (just 1st or 2nd order approximation) we should change it to rodriquez +% approximation. +% use_inertia_matrix: set to true if you have the inertia matrix J for your +% quadrotor +% xa_apo_k: old state vectotr +% zFlag: if sensor measurement is available [gyro, acc, mag] +% dt: dt in s +% z: measurements [gyro, acc, mag] +% q_rotSpeed: process noise gyro +% q_rotAcc: process noise gyro acceleration +% q_acc: process noise acceleration +% q_mag: process noise magnetometer +% r_gyro: measurement noise gyro +% r_accel: measurement noise accel +% r_mag: measurement noise mag +% J: moment of inertia matrix + + +% Output: +% xa_apo: updated state vectotr +% Pa_apo: updated state covariance matrix +% Rot_matrix: rotation matrix +% eulerAngles: euler angles +% debugOutput: not used + + +%% model specific parameters + + + +% compute once the inverse of the Inertia +persistent Ji; +if isempty(Ji) + Ji=single(inv(J)); +end + +%% init +persistent x_apo +if(isempty(x_apo)) + gyro_init=single([0;0;0]); + gyro_acc_init=single([0;0;0]); + acc_init=single([0;0;-9.81]); + mag_init=single([1;0;0]); + x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); + +end + +persistent P_apo +if(isempty(P_apo)) + % P_apo = single(eye(NSTATES) * 1000); + P_apo = single(200*ones(12)); +end + +debugOutput = single(zeros(4,1)); + +%% copy the states +wx= x_apo(1); % x body angular rate +wy= x_apo(2); % y body angular rate +wz= x_apo(3); % z body angular rate + +wax= x_apo(4); % x body angular acceleration +way= x_apo(5); % y body angular acceleration +waz= x_apo(6); % z body angular acceleration + +zex= x_apo(7); % x component gravity vector +zey= x_apo(8); % y component gravity vector +zez= x_apo(9); % z component gravity vector + +mux= x_apo(10); % x component magnetic field vector +muy= x_apo(11); % y component magnetic field vector +muz= x_apo(12); % z component magnetic field vector + + + + +%% prediction section +% compute the apriori state estimate from the previous aposteriori estimate +%body angular accelerations +if (use_inertia_matrix==1) + wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; +else + wak =[wax;way;waz]; +end + +%body angular rates +wk =[wx; wy; wz] + dt*wak; + +%derivative of the prediction rotation matrix +O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; + +%prediction of the earth z vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + zek =(O*dt+single(eye(3)))*[zex;zey;zez]; + +else + zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; + %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double + %precision +end + + + +%prediction of the magnetic vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + muk =(O*dt+single(eye(3)))*[mux;muy;muz]; +else + muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; + %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double + %precision +end + +x_apr=[wk;wak;zek;muk]; + +% compute the apriori error covariance estimate from the previous +%aposteriori estimate + +EZ=[0,zez,-zey; + -zez,0,zex; + zey,-zex,0]'; +MA=[0,muz,-muy; + -muz,0,mux; + muy,-mux,0]'; + +E=single(eye(3)); +Z=single(zeros(3)); + +A_lin=[ Z, E, Z, Z + Z, Z, Z, Z + EZ, Z, O, Z + MA, Z, Z, O]; + +A_lin=eye(12)+A_lin*dt; + +%process covariance matrix + +persistent Q +if (isempty(Q)) + Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... + q_rotAcc,q_rotAcc,q_rotAcc,... + q_acc,q_acc,q_acc,... + q_mag,q_mag,q_mag]); +end + +P_apr=A_lin*P_apo*A_lin'+Q; + + +%% update +if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 + +% R=[r_gyro,0,0,0,0,0,0,0,0; +% 0,r_gyro,0,0,0,0,0,0,0; +% 0,0,r_gyro,0,0,0,0,0,0; +% 0,0,0,r_accel,0,0,0,0,0; +% 0,0,0,0,r_accel,0,0,0,0; +% 0,0,0,0,0,r_accel,0,0,0; +% 0,0,0,0,0,0,r_mag,0,0; +% 0,0,0,0,0,0,0,r_mag,0; +% 0,0,0,0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; + %observation matrix + %[zw;ze;zmk]; + H_k=[ E, Z, Z, Z; + Z, Z, E, Z; + Z, Z, Z, E]; + + y_k=z(1:9)-H_k*x_apr; + + + %S_k=H_k*P_apr*H_k'+R; + S_k=H_k*P_apr*H_k'; + S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; + K_k=(P_apr*H_k'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k)*P_apr; +else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 + + R=[r_gyro,0,0; + 0,r_gyro,0; + 0,0,r_gyro]; + R_v=[r_gyro,r_gyro,r_gyro]; + %observation matrix + + H_k=[ E, Z, Z, Z]; + + y_k=z(1:3)-H_k(1:3,1:12)*x_apr; + + % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; + S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; + K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 + +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_accel,0,0; +% 0,0,0,0,r_accel,0; +% 0,0,0,0,0,r_accel]; + + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, E, Z]; + + y_k=z(1:6)-H_k(1:6,1:12)*x_apr; + + % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_mag,0,0; +% 0,0,0,0,r_mag,0; +% 0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, Z, E]; + + y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; + + %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + x_apo=x_apr; + P_apo=P_apr; + end + end + end +end + + + +%% euler anglels extraction +z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); +m_n_b = x_apo(10:12)./norm(x_apo(10:12)); + +y_n_b=cross(z_n_b,m_n_b); +y_n_b=y_n_b./norm(y_n_b); + +x_n_b=(cross(y_n_b,z_n_b)); +x_n_b=x_n_b./norm(x_n_b); + + +xa_apo=x_apo; +Pa_apo=P_apo; +% rotation matrix from earth to body system +Rot_matrix=[x_n_b,y_n_b,z_n_b]; + + +phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); +theta=-asin(Rot_matrix(1,3)); +psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); +eulerAngles=[phi;theta;psi]; + diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100644 index 0000000000..9ea5203463 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,502 @@ + + + + + false + false + false + option.WorkingFolder.Project + + option.BuildFolder.Project + + + true + true + true + true + option.GlobalDataSyncMethod.SyncAlways + true + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + option.FilePartitionMethod.MapMFileToCFile + true + false + + true + false + true + false + + + + + + + + + true + false + 40000 + 100 + option.TargetLang.C + true + 10 + 200 + 4000 + true + 64 + true + true + option.ConstantInputs.CheckValues + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + option.WorkingFolder.Project + + option.BuildFolder.Specified + codegen + + true + false + false + false + option.DynamicMemoryAllocation.Threshold + 65536 + 4000 + false + option.FilePartitionMethod.SingleFile + true + true + false + option.DataTypeReplacement.CBuiltIn + false + true + option.ParenthesesLevel.Nominal + 31 + $M$N + $M$N + $M$N + $M$N + $M$N + $M$N + emxArray_$M$N + emx$M$N + + true + false + true + true + false + true + + + + + + + + + C89/C90 (ANSI) + true + ARM Compatible + ARM Cortex + true + 8 + 16 + 32 + 32 + 64 + 32 + 64 + 32 + 32 + option.HardwareEndianness.Little + true + false + option.HardwareAtomicIntegerSize.Long + option.HardwareAtomicFloatSize.Double + option.HardwareDivisionRounding.Undefined + Generic + MATLAB Host Computer + false + 8 + 16 + 32 + 64 + 64 + 32 + 64 + 64 + 64 + option.HardwareEndianness.Little + true + true + option.HardwareAtomicIntegerSize.Char + option.HardwareAtomicFloatSize.None + option.HardwareDivisionRounding.Zero + Automatically locate an installed toolchain + Faster Builds + + 40000 + 100 + true + option.TargetLang.C + option.CCompilerOptimization.On + + true + false + make_rtw + default_tmf + + 10 + 200 + 4000 + true + 64 + true + true + false + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + R2012a + true + t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html + /home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html + true + option.VerificationMode.None + false + ${PROJECT_ROOT}/AttitudeEKF_Test.m + ${PROJECT_ROOT}/AttitudeEKF_Test.m + false + false + 1024 + 2048 + AttitudeEKF_mex + AttitudeEKF + option.target.artifact.lib + option.FixedPointTypeProposalMode.ProposeFractionLengths + numerictype([],16,12) + 0 + true + false + false + false + true + false + + option.FixedPointMode.None + false + + + 16 + 4 + 0 + fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128) + option.FixedPointTypeSource.SimAndDerived + + false + false + false + true + + + + + + + + + true + _fixpt + option.DefaultFixedPointSignedness.Automatic + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${PROJECT_ROOT}/AttitudeEKF_Test.m + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + + + + /opt/matlab/r2013b + + + + + + true + false + false + false + false + false + true + false + 3.16.1-1-ARCH + false + true + glnxa64 + true + + + + diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 8d537d6763..9784ea1a16 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -74,8 +75,7 @@ #ifdef __cplusplus extern "C" { #endif -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" +#include "codegen/AttitudeEKF.h" #include "attitude_estimator_ekf_params.h" #ifdef __cplusplus } @@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 14000, + 7200, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); @@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ + float debugOutput[4] = { 0.0f }; + int overloadcounter = 19; /* Initialize filter */ - attitudeKalmanfilter_initialize(); + AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); @@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params; + struct attitude_estimator_ekf_params ekf_params = { 0 }; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -504,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* Call the estimator */ + AttitudeEKF(false, // approx_prediction + (unsigned char)ekf_params.use_moment_inertia, + update_vect, + dt, + z_k, + ekf_params.q[0], // q_rotSpeed, + ekf_params.q[1], // q_rotAcc + ekf_params.q[2], // q_acc + ekf_params.q[3], // q_mag + ekf_params.r[0], // r_gyro + ekf_params.r[1], // r_accel + ekf_params.r[2], // r_mag + ekf_params.moment_inertia_J, + x_aposteriori, + P_aposteriori, + Rot_matrix, + euler, + debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index bc0e3b93a2..5637ec102f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,28 +44,96 @@ /* Extended Kalman Filter covariances */ -/* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); -/* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); -/* gyro measurement noise */ +/** + * Body angular rate process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); + +/** + * Body angular acceleration process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); + +/** + * Acceleration process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); + +/** + * Gyro measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); -/* accel measurement noise */ + +/** + * Accel measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); -/* mag measurement noise */ + +/** + * Mag measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); -/* offset estimation - UNUSED */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); +/** + * Moment of inertia matrix diagonal entry (1, 1) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (2, 2) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (3, 3) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); + +/** + * Moment of inertia enabled in estimator + * + * If set to != 0 the moment of inertia will be used in the estimator + * + * @group attitude_ekf + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_J_EN, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q1 = param_find("EKF_ATT_V3_Q1"); h->q2 = param_find("EKF_ATT_V3_Q2"); h->q3 = param_find("EKF_ATT_V3_Q3"); - h->q4 = param_find("EKF_ATT_V3_Q4"); h->r0 = param_find("EKF_ATT_V4_R0"); h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->r3 = param_find("EKF_ATT_V4_R3"); h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); + h->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + h->use_moment_inertia = param_find("ATT_J_EN"); + return OK; } @@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q1, &(p->q[1])); param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); - param_get(h->q4, &(p->q[4])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); + for (int i = 0; i < 3; i++) { + param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); + } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 5985541cac..5d3b6b2440 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -42,8 +42,10 @@ #include struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; + float r[3]; + float q[4]; + float moment_inertia_J[9]; + int32_t use_moment_inertia; float roll_off; float pitch_off; float yaw_off; @@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3; - param_t q0, q1, q2, q3, q4; + param_t r0, r1, r2; + param_t q0, q1, q2, q3; + param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ + param_t use_moment_inertia; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c new file mode 100644 index 0000000000..68db382cfe --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -0,0 +1,1456 @@ +/* + * AttitudeEKF.c + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +/* Include files */ +#include "AttitudeEKF.h" + +/* Variable Definitions */ +static float Ji[9]; +static boolean_T Ji_not_empty; +static float x_apo[12]; +static float P_apo[144]; +static float Q[144]; +static boolean_T Q_not_empty; + +/* Function Declarations */ +static void AttitudeEKF_init(void); +static void b_mrdivide(const float A[72], const float B[36], float y[72]); +static void inv(const float x[9], float y[9]); +static void mrdivide(const float A[108], const float B[81], float y[108]); +static float norm(const float x[3]); + +/* Function Definitions */ +static void AttitudeEKF_init(void) +{ + int i; + static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + -9.81F, 1.0F, 0.0F, 0.0F }; + + for (i = 0; i < 12; i++) { + x_apo[i] = fv5[i]; + } + + for (i = 0; i < 144; i++) { + P_apo[i] = 200.0F; + } +} + +/* + * + */ +static void b_mrdivide(const float A[72], const float B[36], float y[72]) +{ + float b_A[36]; + signed char ipiv[6]; + int i1; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[72]; + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i1] = B[i1 + 6 * iy]; + } + + ipiv[i1] = (signed char)(1 + i1); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; + } + } + + i1 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i1; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i1 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i1 = 0; i1 < 12; i1++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i1] = A[i1 + 12 * iy]; + } + } + + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i1] = Y[i1 + 6 * iy]; + } + } +} + +/* + * + */ +static void inv(const float x[9], float y[9]) +{ + float b_x[9]; + int p1; + int p2; + int p3; + float absx11; + float absx21; + float absx31; + int itmp; + for (p1 = 0; p1 < 9; p1++) { + b_x[p1] = x[p1]; + } + + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = (real32_T)fabs(x[0]); + absx21 = (real32_T)fabs(x[1]); + absx31 = (real32_T)fabs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + absx11 = b_x[1] / b_x[0]; + b_x[1] /= b_x[0]; + absx21 = b_x[2] / b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= absx11 * b_x[3]; + b_x[5] -= absx21 * b_x[3]; + b_x[7] -= absx11 * b_x[6]; + b_x[8] -= absx21 * b_x[6]; + if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + b_x[1] = absx21; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + absx11 = b_x[5] / b_x[4]; + b_x[5] /= b_x[4]; + b_x[8] -= absx11 * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0F / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; +} + +/* + * + */ +static void mrdivide(const float A[108], const float B[81], float y[108]) +{ + float b_A[81]; + signed char ipiv[9]; + int i0; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[108]; + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i0] = B[i0 + 9 * iy]; + } + + ipiv[i0] = (signed char)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i0 = 0; i0 < 12; i0++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i0] = A[i0 + 12 * iy]; + } + } + + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i0] = Y[i0 + 9 * iy]; + } + } +} + +/* + * + */ +static float norm(const float x[3]) +{ + float y; + float scale; + int k; + float absxk; + float t; + y = 0.0F; + scale = 1.17549435E-38F; + for (k = 0; k < 3; k++) { + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + return scale * (real32_T)sqrt(y); +} + +/* + * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + * = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + */ +void AttitudeEKF(unsigned char approx_prediction, unsigned char + use_inertia_matrix, const unsigned char zFlag[3], float dt, + const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, + float q_mag, float r_gyro, float r_accel, float r_mag, const + float J[9], float xa_apo[12], float Pa_apo[144], float + Rot_matrix[9], float eulerAngles[3], float debugOutput[4]) +{ + int i; + float fv0[3]; + int r2; + float zek[3]; + float muk[3]; + float b_muk[3]; + float c_muk[3]; + float fv1[3]; + float wak[3]; + float O[9]; + float b_O[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float fv2[3]; + float maxval; + int r1; + float fv3[9]; + float fv4[3]; + float x_apr[12]; + signed char I[144]; + static float A_lin[144]; + static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static float b_A_lin[144]; + float v[12]; + static float P_apr[144]; + float a[108]; + static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float S_k[81]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[9]; + float K_k[108]; + float b_S_k[36]; + static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float c_r_gyro[3]; + float B[36]; + int r3; + float a21; + float Y[36]; + float d_a[72]; + static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 }; + + float d_r_gyro[6]; + float c_S_k[6]; + float b_K_k[72]; + static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; + + static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1 }; + + float b_z[6]; + + /* LQG Postion Estimator and Controller */ + /* Observer: */ + /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ + /* x[n+1|n] = Ax[n|n] + Bu[n] */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ */ + /* */ + /* */ + /* Arguments: */ + /* approx_prediction: if 1 then the exponential map is approximated with a */ + /* first order taylor approximation. has at the moment not a big influence */ + /* (just 1st or 2nd order approximation) we should change it to rodriquez */ + /* approximation. */ + /* use_inertia_matrix: set to true if you have the inertia matrix J for your */ + /* quadrotor */ + /* xa_apo_k: old state vectotr */ + /* zFlag: if sensor measurement is available [gyro, acc, mag] */ + /* dt: dt in s */ + /* z: measurements [gyro, acc, mag] */ + /* q_rotSpeed: process noise gyro */ + /* q_rotAcc: process noise gyro acceleration */ + /* q_acc: process noise acceleration */ + /* q_mag: process noise magnetometer */ + /* r_gyro: measurement noise gyro */ + /* r_accel: measurement noise accel */ + /* r_mag: measurement noise mag */ + /* J: moment of inertia matrix */ + /* Output: */ + /* xa_apo: updated state vectotr */ + /* Pa_apo: updated state covariance matrix */ + /* Rot_matrix: rotation matrix */ + /* eulerAngles: euler angles */ + /* debugOutput: not used */ + /* % model specific parameters */ + /* compute once the inverse of the Inertia */ + /* 'AttitudeEKF:48' if isempty(Ji) */ + if (!Ji_not_empty) { + /* 'AttitudeEKF:49' Ji=single(inv(J)); */ + inv(J, Ji); + Ji_not_empty = TRUE; + } + + /* % init */ + /* 'AttitudeEKF:54' if(isempty(x_apo)) */ + /* 'AttitudeEKF:64' if(isempty(P_apo)) */ + /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ + for (i = 0; i < 4; i++) { + debugOutput[i] = 0.0F; + } + + /* % copy the states */ + /* 'AttitudeEKF:72' wx= x_apo(1); */ + /* x body angular rate */ + /* 'AttitudeEKF:73' wy= x_apo(2); */ + /* y body angular rate */ + /* 'AttitudeEKF:74' wz= x_apo(3); */ + /* z body angular rate */ + /* 'AttitudeEKF:76' wax= x_apo(4); */ + /* x body angular acceleration */ + /* 'AttitudeEKF:77' way= x_apo(5); */ + /* y body angular acceleration */ + /* 'AttitudeEKF:78' waz= x_apo(6); */ + /* z body angular acceleration */ + /* 'AttitudeEKF:80' zex= x_apo(7); */ + /* x component gravity vector */ + /* 'AttitudeEKF:81' zey= x_apo(8); */ + /* y component gravity vector */ + /* 'AttitudeEKF:82' zez= x_apo(9); */ + /* z component gravity vector */ + /* 'AttitudeEKF:84' mux= x_apo(10); */ + /* x component magnetic field vector */ + /* 'AttitudeEKF:85' muy= x_apo(11); */ + /* y component magnetic field vector */ + /* 'AttitudeEKF:86' muz= x_apo(12); */ + /* z component magnetic field vector */ + /* % prediction section */ + /* compute the apriori state estimate from the previous aposteriori estimate */ + /* body angular accelerations */ + /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ + if (use_inertia_matrix == 1) { + /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ + fv0[0] = x_apo[3]; + fv0[1] = x_apo[4]; + fv0[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += J[r2 + 3 * i] * fv0[i]; + } + } + + muk[0] = x_apo[3]; + muk[1] = x_apo[4]; + muk[2] = x_apo[5]; + b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; + b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; + b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; + for (r2 = 0; r2 < 3; r2++) { + c_muk[r2] = -b_muk[r2]; + } + + fv1[0] = x_apo[3]; + fv1[1] = x_apo[4]; + fv1[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + fv0[r2] = 0.0F; + for (i = 0; i < 3; i++) { + fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; + } + + wak[r2] = fv1[r2] + fv0[r2] * dt; + } + } else { + /* 'AttitudeEKF:96' else */ + /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ + wak[0] = x_apo[3]; + wak[1] = x_apo[4]; + wak[2] = x_apo[5]; + } + + /* body angular rates */ + /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_apo[2]; + O[2] = x_apo[1]; + O[3] = x_apo[2]; + O[4] = 0.0F; + O[5] = -x_apo[0]; + O[6] = -x_apo[1]; + O[7] = x_apo[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'AttitudeEKF:107' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += b_O[r2 + 3 * i] * fv2[i]; + } + } + } else { + /* 'AttitudeEKF:112' else */ + /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += fv3[r2 + 3 * i] * fv2[i]; + } + } + + /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ + /* precision */ + } + + /* prediction of the magnetic vector */ + /* 'AttitudeEKF:121' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += b_O[r2 + 3 * i] * fv4[i]; + } + } + } else { + /* 'AttitudeEKF:125' else */ + /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += fv3[r2 + 3 * i] * fv4[i]; + } + } + + /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ + /* precision */ + } + + /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ + x_apr[0] = x_apo[0] + dt * wak[0]; + x_apr[1] = x_apo[1] + dt * wak[1]; + x_apr[2] = x_apo[2] + dt * wak[2]; + for (i = 0; i < 3; i++) { + x_apr[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 6] = zek[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 9] = muk[i]; + } + + /* compute the apriori error covariance estimate from the previous */ + /* aposteriori estimate */ + /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ + /* 'AttitudeEKF:137' -zez,0,zex; */ + /* 'AttitudeEKF:138' zey,-zex,0]'; */ + /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ + /* 'AttitudeEKF:140' -muz,0,mux; */ + /* 'AttitudeEKF:141' muy,-mux,0]'; */ + /* 'AttitudeEKF:143' E=single(eye(3)); */ + /* 'AttitudeEKF:144' Z=single(zeros(3)); */ + /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ + /* 'AttitudeEKF:147' Z, Z, Z, Z */ + /* 'AttitudeEKF:148' EZ, Z, O, Z */ + /* 'AttitudeEKF:149' MA, Z, Z, O]; */ + /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + for (r2 = 0; r2 < 3; r2++) { + A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; + } + + for (r2 = 0; r2 < 3; r2++) { + A_lin[(r2 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_apo[8]; + A_lin[8] = -x_apo[7]; + A_lin[18] = -x_apo[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_apo[6]; + A_lin[30] = x_apo[7]; + A_lin[31] = -x_apo[6]; + A_lin[32] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_apo[11]; + A_lin[11] = -x_apo[10]; + A_lin[21] = -x_apo[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_apo[9]; + A_lin[33] = x_apo[10]; + A_lin[34] = -x_apo[9]; + A_lin[35] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; + } + } + + /* process covariance matrix */ + /* 'AttitudeEKF:156' if (isempty(Q)) */ + if (!Q_not_empty) { + /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ + /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ + /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ + /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ + v[0] = q_rotSpeed; + v[1] = q_rotSpeed; + v[2] = q_rotSpeed; + v[3] = q_rotAcc; + v[4] = q_rotAcc; + v[5] = q_rotAcc; + v[6] = q_acc; + v[7] = q_acc; + v[8] = q_acc; + v[9] = q_mag; + v[10] = q_mag; + v[11] = q_mag; + memset(&Q[0], 0, 144U * sizeof(float)); + for (i = 0; i < 12; i++) { + Q[i + 12 * i] = v[i]; + } + + Q_not_empty = TRUE; + } + + /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + A_lin[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; + } + + P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; + } + } + + /* % update */ + /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0,0,0,0; */ + /* 0,0,0,r_accel,0,0,0,0,0; */ + /* 0,0,0,0,r_accel,0,0,0,0; */ + /* 0,0,0,0,0,r_accel,0,0,0; */ + /* 0,0,0,0,0,0,r_mag,0,0; */ + /* 0,0,0,0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:182' Z, Z, E, Z; */ + /* 'AttitudeEKF:183' Z, Z, Z, E]; */ + /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ + /* S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 12; i++) { + a[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + S_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ + b_r_gyro[0] = r_gyro; + b_r_gyro[1] = r_gyro; + b_r_gyro[2] = r_gyro; + b_r_gyro[3] = r_accel; + b_r_gyro[4] = r_accel; + b_r_gyro[5] = r_accel; + b_r_gyro[6] = r_mag; + b_r_gyro[7] = r_mag; + b_r_gyro[8] = r_mag; + for (r2 = 0; r2 < 9; r2++) { + b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; + } + + for (r2 = 0; r2 < 9; r2++) { + S_k[10 * r2] = b_O[r2]; + } + + /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 9; i++) { + a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + mrdivide(a, S_k, K_k); + + /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 9; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; + } + + b_O[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 9; i++) { + maxval += K_k[r2 + 12 * i] * b_O[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 9; r1++) { + maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:196' else */ + /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:200' 0,r_gyro,0; */ + /* 'AttitudeEKF:201' 0,0,r_gyro]; */ + /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ + /* observation matrix */ + /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 3; i++) { + O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ + c_r_gyro[0] = r_gyro; + c_r_gyro[1] = r_gyro; + c_r_gyro[2] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + O[r2 << 2] = b_muk[r2]; + } + + /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[r2 + 3 * i]; + } + + for (i = 0; i < 12; i++) { + B[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(b_O[0]); + a21 = (real32_T)fabs(b_O[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(b_O[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_O[r2] /= b_O[r1]; + b_O[r3] /= b_O[r1]; + b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; + b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; + b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; + b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; + if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + b_O[3 + r3] /= b_O[3 + r2]; + b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; + for (i = 0; i < 12; i++) { + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * + b_O[3 + r3]; + Y[2 + 3 * i] /= b_O[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; + Y[1 + 3 * i] /= b_O[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; + Y[3 * i] /= b_O[r1]; + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; + } + } + + /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; + } + + b_muk[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 3; i++) { + maxval += b_S_k[r2 + 12 * i] * b_muk[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:217' else */ + /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_accel,0,0; */ + /* 0,0,0,0,r_accel,0; */ + /* 0,0,0,0,0,r_accel]; */ + /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ + /* observation matrix */ + /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:231' Z, Z, E, Z]; */ + /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_accel; + d_r_gyro[4] = r_accel; + d_r_gyro[5] = r_accel; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 6; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; + } + + d_r_gyro[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:243' else */ + /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_mag,0,0; */ + /* 0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:255' Z, Z, Z, E]; */ + /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_mag; + d_r_gyro[4] = r_mag; + d_r_gyro[5] = r_mag; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2] = z[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2 + 3] = z[6 + r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = 0.0F; + for (i = 0; i < 12; i++) { + c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; + } + + b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * b_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:267' else */ + /* 'AttitudeEKF:268' x_apo=x_apr; */ + for (i = 0; i < 12; i++) { + x_apo[i] = x_apr[i]; + } + + /* 'AttitudeEKF:269' P_apo=P_apr; */ + memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + maxval = norm(*(float (*)[3])&x_apo[6]); + a21 = norm(*(float (*)[3])&x_apo[9]); + for (i = 0; i < 3; i++) { + /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + muk[i] = -x_apo[i + 6] / maxval; + zek[i] = x_apo[i + 9] / a21; + } + + /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ + wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; + wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; + wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; + + /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ + maxval = norm(wak); + for (r2 = 0; r2 < 3; r2++) { + wak[r2] /= maxval; + } + + /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ + zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; + zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; + zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; + + /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ + maxval = norm(zek); + for (r2 = 0; r2 < 3; r2++) { + zek[r2] /= maxval; + } + + /* 'AttitudeEKF:288' xa_apo=x_apo; */ + for (i = 0; i < 12; i++) { + xa_apo[i] = x_apo[i]; + } + + /* 'AttitudeEKF:289' Pa_apo=P_apo; */ + memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); + + /* rotation matrix from earth to body system */ + /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (r2 = 0; r2 < 3; r2++) { + Rot_matrix[r2] = zek[r2]; + Rot_matrix[3 + r2] = wak[r2]; + Rot_matrix[6 + r2] = muk[r2]; + } + + /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); +} + +void AttitudeEKF_initialize(void) +{ + Q_not_empty = FALSE; + Ji_not_empty = FALSE; + AttitudeEKF_init(); +} + +void AttitudeEKF_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (AttitudeEKF.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h new file mode 100644 index 0000000000..7094da1da5 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -0,0 +1,26 @@ +/* + * AttitudeEKF.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_H__ +#define __ATTITUDEEKF_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "AttitudeEKF_types.h" + +/* Function Declarations */ +extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); +extern void AttitudeEKF_initialize(void); +extern void AttitudeEKF_terminate(void); +#endif +/* End of code generation (AttitudeEKF.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h new file mode 100644 index 0000000000..3f7522ffa0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -0,0 +1,17 @@ +/* + * AttitudeEKF_types.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_TYPES_H__ +#define __ATTITUDEEKF_TYPES_H__ + +/* Include files */ +#include "rtwtypes.h" + +#endif +/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a8..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/* - * attitudeKalmanfilter.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" -#include "norm.h" -#include "cross.h" -#include "eye.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) -{ - real32_T y; - int32_T b_u0; - int32_T b_u1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - if (u0 > 0.0F) { - b_u0 = 1; - } else { - b_u0 = -1; - } - - if (u1 > 0.0F) { - b_u1 = 1; - } else { - b_u1 = -1; - } - - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); - } else if (u1 == 0.0F) { - if (u0 > 0.0F) { - y = RT_PIF / 2.0F; - } else if (u0 < 0.0F) { - y = -(RT_PIF / 2.0F); - } else { - y = 0.0F; - } - } else { - y = (real32_T)atan2(u0, u1); - } - - return y; -} - -/* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) - */ -void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const - real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T - P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T - eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) -{ - real32_T wak[3]; - real32_T O[9]; - real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[9]; - real32_T x_n_b[3]; - real32_T b_x_aposteriori_k[3]; - real32_T z_n_b[3]; - real32_T c_a[3]; - real32_T d_a[3]; - int32_T i0; - real32_T x_apriori[12]; - real_T dv1[144]; - real32_T A_lin[144]; - static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T b_A_lin[144]; - real32_T b_q[144]; - real32_T c_A_lin[144]; - real32_T d_A_lin[144]; - real32_T e_A_lin[144]; - int32_T i1; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T K_k[108]; - real32_T fv0[81]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T b_r[81]; - real32_T fv1[81]; - real32_T f0; - real32_T c_P_apriori[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T fv2[36]; - static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T c_r[9]; - real32_T b_K_k[36]; - real32_T d_P_apriori[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0 }; - - real32_T c_K_k[72]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0 }; - - real32_T b_z[6]; - static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1 }; - - static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - - real32_T fv3[6]; - real32_T c_z[6]; - - /* Extended Attitude Kalmanfilter */ - /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ - /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ - /* */ - /* Example.... */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* coder.varsize('udpIndVect', [9,1], [1,0]) */ - /* udpIndVect=find(updVect); */ - /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* observation matrix */ - /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ - /* % prediction section */ - /* body angular accelerations */ - /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ - wak[0] = x_aposteriori_k[3]; - wak[1] = x_aposteriori_k[4]; - wak[2] = x_aposteriori_k[5]; - - /* body angular rates */ - /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_aposteriori_k[2]; - O[2] = x_aposteriori_k[1]; - O[3] = x_aposteriori_k[2]; - O[4] = 0.0F; - O[5] = -x_aposteriori_k[0]; - O[6] = -x_aposteriori_k[1]; - O[7] = x_aposteriori_k[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* prediction of the magnetic vector */ - /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:66' -zez,0,zex; */ - /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:69' -muz,0,mux; */ - /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:74' E=eye(3); */ - /* 'attitudeKalmanfilter:76' Z=zeros(3); */ - /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[0]; - x_n_b[1] = x_aposteriori_k[1]; - x_n_b[2] = x_aposteriori_k[2]; - b_x_aposteriori_k[0] = x_aposteriori_k[6]; - b_x_aposteriori_k[1] = x_aposteriori_k[7]; - b_x_aposteriori_k[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; - for (i = 0; i < 3; i++) { - c_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; - } - - d_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; - } - - x_apriori[i] = x_n_b[i] + dt * wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 6] = c_a[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 9] = d_a[i]; - } - - /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ - /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; - } - - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_aposteriori_k[8]; - A_lin[8] = -x_aposteriori_k[7]; - A_lin[18] = -x_aposteriori_k[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_aposteriori_k[6]; - A_lin[30] = x_aposteriori_k[7]; - A_lin[31] = -x_aposteriori_k[6]; - A_lin[32] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_aposteriori_k[11]; - A_lin[11] = -x_aposteriori_k[10]; - A_lin[21] = -x_aposteriori_k[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_aposteriori_k[9]; - A_lin[33] = x_aposteriori_k[7]; - A_lin[34] = -x_aposteriori_k[9]; - A_lin[35] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * - dt; - } - } - - /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ - /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ - /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ - /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ - /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ - /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - b_q[0] = q[0]; - b_q[12] = 0.0F; - b_q[24] = 0.0F; - b_q[36] = 0.0F; - b_q[48] = 0.0F; - b_q[60] = 0.0F; - b_q[72] = 0.0F; - b_q[84] = 0.0F; - b_q[96] = 0.0F; - b_q[108] = 0.0F; - b_q[120] = 0.0F; - b_q[132] = 0.0F; - b_q[1] = 0.0F; - b_q[13] = q[0]; - b_q[25] = 0.0F; - b_q[37] = 0.0F; - b_q[49] = 0.0F; - b_q[61] = 0.0F; - b_q[73] = 0.0F; - b_q[85] = 0.0F; - b_q[97] = 0.0F; - b_q[109] = 0.0F; - b_q[121] = 0.0F; - b_q[133] = 0.0F; - b_q[2] = 0.0F; - b_q[14] = 0.0F; - b_q[26] = q[0]; - b_q[38] = 0.0F; - b_q[50] = 0.0F; - b_q[62] = 0.0F; - b_q[74] = 0.0F; - b_q[86] = 0.0F; - b_q[98] = 0.0F; - b_q[110] = 0.0F; - b_q[122] = 0.0F; - b_q[134] = 0.0F; - b_q[3] = 0.0F; - b_q[15] = 0.0F; - b_q[27] = 0.0F; - b_q[39] = q[1]; - b_q[51] = 0.0F; - b_q[63] = 0.0F; - b_q[75] = 0.0F; - b_q[87] = 0.0F; - b_q[99] = 0.0F; - b_q[111] = 0.0F; - b_q[123] = 0.0F; - b_q[135] = 0.0F; - b_q[4] = 0.0F; - b_q[16] = 0.0F; - b_q[28] = 0.0F; - b_q[40] = 0.0F; - b_q[52] = q[1]; - b_q[64] = 0.0F; - b_q[76] = 0.0F; - b_q[88] = 0.0F; - b_q[100] = 0.0F; - b_q[112] = 0.0F; - b_q[124] = 0.0F; - b_q[136] = 0.0F; - b_q[5] = 0.0F; - b_q[17] = 0.0F; - b_q[29] = 0.0F; - b_q[41] = 0.0F; - b_q[53] = 0.0F; - b_q[65] = q[1]; - b_q[77] = 0.0F; - b_q[89] = 0.0F; - b_q[101] = 0.0F; - b_q[113] = 0.0F; - b_q[125] = 0.0F; - b_q[137] = 0.0F; - b_q[6] = 0.0F; - b_q[18] = 0.0F; - b_q[30] = 0.0F; - b_q[42] = 0.0F; - b_q[54] = 0.0F; - b_q[66] = 0.0F; - b_q[78] = q[2]; - b_q[90] = 0.0F; - b_q[102] = 0.0F; - b_q[114] = 0.0F; - b_q[126] = 0.0F; - b_q[138] = 0.0F; - b_q[7] = 0.0F; - b_q[19] = 0.0F; - b_q[31] = 0.0F; - b_q[43] = 0.0F; - b_q[55] = 0.0F; - b_q[67] = 0.0F; - b_q[79] = 0.0F; - b_q[91] = q[2]; - b_q[103] = 0.0F; - b_q[115] = 0.0F; - b_q[127] = 0.0F; - b_q[139] = 0.0F; - b_q[8] = 0.0F; - b_q[20] = 0.0F; - b_q[32] = 0.0F; - b_q[44] = 0.0F; - b_q[56] = 0.0F; - b_q[68] = 0.0F; - b_q[80] = 0.0F; - b_q[92] = 0.0F; - b_q[104] = q[2]; - b_q[116] = 0.0F; - b_q[128] = 0.0F; - b_q[140] = 0.0F; - b_q[9] = 0.0F; - b_q[21] = 0.0F; - b_q[33] = 0.0F; - b_q[45] = 0.0F; - b_q[57] = 0.0F; - b_q[69] = 0.0F; - b_q[81] = 0.0F; - b_q[93] = 0.0F; - b_q[105] = 0.0F; - b_q[117] = q[3]; - b_q[129] = 0.0F; - b_q[141] = 0.0F; - b_q[10] = 0.0F; - b_q[22] = 0.0F; - b_q[34] = 0.0F; - b_q[46] = 0.0F; - b_q[58] = 0.0F; - b_q[70] = 0.0F; - b_q[82] = 0.0F; - b_q[94] = 0.0F; - b_q[106] = 0.0F; - b_q[118] = 0.0F; - b_q[130] = q[3]; - b_q[142] = 0.0F; - b_q[11] = 0.0F; - b_q[23] = 0.0F; - b_q[35] = 0.0F; - b_q[47] = 0.0F; - b_q[59] = 0.0F; - b_q[71] = 0.0F; - b_q[83] = 0.0F; - b_q[95] = 0.0F; - b_q[107] = 0.0F; - b_q[119] = 0.0F; - b_q[131] = 0.0F; - b_q[143] = q[3]; - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * - i0]; - } - - c_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 12; i0++) { - d_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - e_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; - } - } - - /* % update */ - /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:112' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ - /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ - /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 - + 12 * i0]; - } - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } - - b_r[0] = r[0]; - b_r[9] = 0.0F; - b_r[18] = 0.0F; - b_r[27] = 0.0F; - b_r[36] = 0.0F; - b_r[45] = 0.0F; - b_r[54] = 0.0F; - b_r[63] = 0.0F; - b_r[72] = 0.0F; - b_r[1] = 0.0F; - b_r[10] = r[0]; - b_r[19] = 0.0F; - b_r[28] = 0.0F; - b_r[37] = 0.0F; - b_r[46] = 0.0F; - b_r[55] = 0.0F; - b_r[64] = 0.0F; - b_r[73] = 0.0F; - b_r[2] = 0.0F; - b_r[11] = 0.0F; - b_r[20] = r[0]; - b_r[29] = 0.0F; - b_r[38] = 0.0F; - b_r[47] = 0.0F; - b_r[56] = 0.0F; - b_r[65] = 0.0F; - b_r[74] = 0.0F; - b_r[3] = 0.0F; - b_r[12] = 0.0F; - b_r[21] = 0.0F; - b_r[30] = r[1]; - b_r[39] = 0.0F; - b_r[48] = 0.0F; - b_r[57] = 0.0F; - b_r[66] = 0.0F; - b_r[75] = 0.0F; - b_r[4] = 0.0F; - b_r[13] = 0.0F; - b_r[22] = 0.0F; - b_r[31] = 0.0F; - b_r[40] = r[1]; - b_r[49] = 0.0F; - b_r[58] = 0.0F; - b_r[67] = 0.0F; - b_r[76] = 0.0F; - b_r[5] = 0.0F; - b_r[14] = 0.0F; - b_r[23] = 0.0F; - b_r[32] = 0.0F; - b_r[41] = 0.0F; - b_r[50] = r[1]; - b_r[59] = 0.0F; - b_r[68] = 0.0F; - b_r[77] = 0.0F; - b_r[6] = 0.0F; - b_r[15] = 0.0F; - b_r[24] = 0.0F; - b_r[33] = 0.0F; - b_r[42] = 0.0F; - b_r[51] = 0.0F; - b_r[60] = r[2]; - b_r[69] = 0.0F; - b_r[78] = 0.0F; - b_r[7] = 0.0F; - b_r[16] = 0.0F; - b_r[25] = 0.0F; - b_r[34] = 0.0F; - b_r[43] = 0.0F; - b_r[52] = 0.0F; - b_r[61] = 0.0F; - b_r[70] = r[2]; - b_r[79] = 0.0F; - b_r[8] = 0.0F; - b_r[17] = 0.0F; - b_r[26] = 0.0F; - b_r[35] = 0.0F; - b_r[44] = 0.0F; - b_r[53] = 0.0F; - b_r[62] = 0.0F; - b_r[71] = 0.0F; - b_r[80] = r[2]; - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv1, K_k); - - /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - O[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * O[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:138' else */ - /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ - /* 'attitudeKalmanfilter:142' 0,r(1),0; */ - /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - c_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv3[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * - i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - O[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; - } - } - } - - c_r[0] = r[0]; - c_r[3] = 0.0F; - c_r[6] = 0.0F; - c_r[1] = 0.0F; - c_r[4] = r[0]; - c_r[7] = 0.0F; - c_r[2] = 0.0F; - c_r[5] = 0.0F; - c_r[8] = r[0]; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; - } - } - - b_mrdivide(c_P_apriori, a, b_K_k); - - /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; - } - - x_n_b[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:156' else */ - /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) - { - /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:159' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ - /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ - /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv5[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[1]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[1]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[1]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 6; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; - } - - b_z[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * b_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:181' else */ - /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) - { - /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv7[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * - i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[2]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[2]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[2]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - b_z[i] = z[i]; - } - - for (i = 0; i < 3; i++) { - b_z[i + 3] = z[i + 6]; - } - - for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; - } - - c_z[i] = b_z[i] - fv3[i]; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * c_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:202' else */ - /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ - for (i = 0; i < 12; i++) { - x_aposteriori[i] = x_apriori[i]; - } - - /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ - memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = -x_aposteriori[i + 6]; - } - - rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - - /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), wak); - - /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - cross(z_n_b, x_n_b, wak); - - /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - rdivide(x_n_b, norm(wak), wak); - - /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(wak, z_n_b, x_n_b); - - /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ - for (i = 0; i < 3; i++) { - b_x_aposteriori_k[i] = x_n_b[i]; - } - - rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - - /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (i = 0; i < 3; i++) { - Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = wak[i]; - Rot_matrix[6 + i] = z_n_b[i]; - } - - /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); -} - -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a91..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_H__ -#define __ATTITUDEKALMANFILTER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); -#endif -/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.c - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be663..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.h - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ -#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_initialize(void); -#endif -/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f97274196..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.c - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a50244..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.h - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ -#define __ATTITUDEKALMANFILTER_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_terminate(void); -#endif -/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * attitudeKalmanfilter_types.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ -#define __ATTITUDEKALMANFILTER_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002b..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * cross.c - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "cross.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) -{ - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; -} - -/* End of code generation (cross.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684a..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * cross.h - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __CROSS_H__ -#define __CROSS_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); -#endif -/* End of code generation (cross.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -1,51 +0,0 @@ -/* - * eye.c - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "eye.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_eye(real_T I[144]) -{ - int32_T i; - memset(&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; - } -} - -/* - * - */ -void eye(real_T I[9]) -{ - int32_T i; - memset(&I[0], 0, 9U * sizeof(real_T)); - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1.0; - } -} - -/* End of code generation (eye.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe76717..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * eye.h - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __EYE_H__ -#define __EYE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_eye(real_T I[144]); -extern void eye(real_T I[9]); -#endif -/* End of code generation (eye.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4f..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -1,357 +0,0 @@ -/* - * mrdivide.c - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) -{ - real32_T b_A[9]; - int32_T rtemp; - int32_T k; - real32_T b_B[36]; - int32_T r1; - int32_T r2; - int32_T r3; - real32_T maxval; - real32_T a21; - real32_T Y[36]; - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; - } - } - - for (rtemp = 0; rtemp < 12; rtemp++) { - for (k = 0; k < 3; k++) { - b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(b_A[0]); - a21 = (real32_T)fabs(b_A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(b_A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_A[r2] /= b_A[r1]; - b_A[r3] /= b_A[r1]; - b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; - b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; - b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; - b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; - } - - b_A[3 + r3] /= b_A[3 + r2]; - b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; - for (k = 0; k < 12; k++) { - Y[3 * k] = b_B[r1 + 3 * k]; - Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; - Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 - + r3]; - Y[2 + 3 * k] /= b_A[6 + r3]; - Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; - Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; - Y[1 + 3 * k] /= b_A[3 + r2]; - Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; - Y[3 * k] /= b_A[r1]; - } - - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 12; k++) { - y[k + 12 * rtemp] = Y[rtemp + 3 * k]; - } - } -} - -/* - * - */ -void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) -{ - real32_T b_A[36]; - int8_T ipiv[6]; - int32_T i3; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[72]; - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * i3] = B[i3 + 6 * iy]; - } - - ipiv[i3] = (int8_T)(1 + i3); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 6; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 6; - iy += 6; - } - } - - i3 = (c - j) + 6; - for (jy = c + 1; jy + 1 <= i3; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 6; - for (k = 1; k <= 5 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i3 = (iy - j) + 12; - for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 6; - iy += 6; - } - } - - for (i3 = 0; i3 < 12; i3++) { - for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * i3] = A[i3 + 12 * iy]; - } - } - - for (jy = 0; jy < 6; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 6 * j]; - Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; - Y[(ipiv[jy] + 6 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 0; k < 6; k++) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 7; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 5; k > -1; k += -1) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i3] = Y[i3 + 6 * iy]; - } - } -} - -/* - * - */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) -{ - real32_T b_A[81]; - int8_T ipiv[9]; - int32_T i2; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[108]; - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * i2] = B[i2 + 9 * iy]; - } - - ipiv[i2] = (int8_T)(1 + i2); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; - } - } - - i2 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i2; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i2 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - for (i2 = 0; i2 < 12; i2++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * i2] = A[i2 + 12 * iy]; - } - } - - for (jy = 0; jy < 9; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 9 * j]; - Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; - Y[(ipiv[jy] + 9 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 10; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i2] = Y[i2 + 9 * iy]; - } - } -} - -/* End of code generation (mrdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51ff..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * mrdivide.h - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __MRDIVIDE_H__ -#define __MRDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); -extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); -#endif -/* End of code generation (mrdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -1,54 +0,0 @@ -/* - * norm.c - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "norm.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -real32_T norm(const real32_T x[3]) -{ - real32_T y; - real32_T scale; - int32_T k; - real32_T absxk; - real32_T t; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = 0; k < 3; k++) { - absxk = (real32_T)fabs(x[k]); - if (absxk > scale) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt(y); -} - -/* End of code generation (norm.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b571..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * norm.h - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __NORM_H__ -#define __NORM_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern real32_T norm(const real32_T x[3]); -#endif -/* End of code generation (norm.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * rdivide.c - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) -{ - int32_T i; - for (i = 0; i < 3; i++) { - z[i] = x[i] / y; - } -} - -/* End of code generation (rdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe28..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * rdivide.h - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RDIVIDE_H__ -#define __RDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); -#endif -/* End of code generation (rdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d1044..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd02..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 3564983630..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * rt_defines.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_DEFINES_H__ -#define __RT_DEFINES_H__ - -#include - -#define RT_PI 3.14159265358979323846 -#define RT_PIF 3.1415927F -#define RT_LN_10 2.30258509299404568402 -#define RT_LN_10F 2.3025851F -#define RT_LOG10E 0.43429448190325182765 -#define RT_LOG10EF 0.43429449F -#define RT_E 2.7182818284590452354 -#define RT_EF 2.7182817F -#endif -/* End of code generation (rt_defines.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d94..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100755 new mode 100644 index 9a5c96267e..b5a02a7a6d --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -1,159 +1,160 @@ -/* - * rtwtypes.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ +/* + * rtwtypes.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 99d0c5bf28..749b0a91c3 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -39,16 +39,6 @@ MODULE_COMMAND = attitude_estimator_ekf SRCS = attitude_estimator_ekf_main.cpp \ attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c + codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index db6773b8a0..c0b1bb4040 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,7 +83,7 @@ #include #include #include -#include +#include #include "estimator_23states.h" diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp new file mode 100644 index 0000000000..f543c02f9c --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,325 @@ +/* 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 mc_att_control_base.cpp + * + * @author Roman Bapst + * + */ + +#include "fw_att_control_base.h" +#include +#include +#include + +using namespace std; + +FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : + + _task_should_exit(false), _task_running(false), _control_task(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { + /* safely initialize structs */ + _att = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { + +} + +void FixedwingAttitudeControlBase::control_attitude() { + bool lock_integrator = false; + static int loop_counter = 0; + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is not updating, we assume the normal average speed */ + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _parameters.airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); + + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; + att_sp.thrust = throttle_sp; + + } + + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { + if (_debug && loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } + } + + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, + speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, + _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!isfinite(roll_u)) { + _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + + if (_debug && loop_counter % 10 == 0) { + warnx("roll_u %.4f", (double) roll_u); + } + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; + if (!isfinite(pitch_u)) { + _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, (double) airspeed_scaling, + (double) roll_sp, (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); + } + } + + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + if (!isfinite(yaw_u)) { + _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double) yaw_u); + } + } + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + if (_debug && loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double) throttle_sp); + } + } + } else { + perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); + } + } + +} + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { + // watch out, still need to see where we modify attitude for the tailsitter case + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _att.q[0] = quat(0); + _att.q[1] = quat(1); + _att.q[2] = quat(2); + _att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _att.R[0][0] = Rot(0,0); + _att.R[1][0] = Rot(1,0); + _att.R[2][0] = Rot(2,0); + _att.R[0][1] = Rot(0,1); + _att.R[1][1] = Rot(1,1); + _att.R[2][1] = Rot(2,1); + _att.R[0][2] = Rot(0,2); + _att.R[1][2] = Rot(1,2); + _att.R[2][2] = Rot(2,2); + + _att.R_valid = true; +} +void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + _att.rollspeed = angular_rate(0); + _att.pitchspeed = angular_rate(1); + _att.yawspeed = angular_rate(2); +} +void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _att_sp.roll_body = control_attitude_thrust_reference(0); + _att_sp.pitch_body = control_attitude_thrust_reference(1); + _att_sp.yaw_body = control_attitude_thrust_reference(2); + _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + _att_sp.R_body[0][0] = Rot_sp(0,0); + _att_sp.R_body[1][0] = Rot_sp(1,0); + _att_sp.R_body[2][0] = Rot_sp(2,0); + _att_sp.R_body[0][1] = Rot_sp(0,1); + _att_sp.R_body[1][1] = Rot_sp(1,1); + _att_sp.R_body[2][1] = Rot_sp(2,1); + _att_sp.R_body[0][2] = Rot_sp(0,2); + _att_sp.R_body[1][2] = Rot_sp(1,2); + _att_sp.R_body[2][2] = Rot_sp(2,2); +} +void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h new file mode 100644 index 0000000000..bde1b755f5 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,151 @@ +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +/* 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 fw_att_control_base.h + * + * @author Roman Bapst + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor + */ + ~FixedwingAttitudeControlBase(); + + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_ff; + float y_roll_feedforward; + float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + } _parameters; /**< local copies of interesting parameters */ + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + void control_attitude(); + + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +}; + +#endif /* FW_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 83fe255711..57c1e72f3c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -75,7 +75,7 @@ #include #include #include -#include +#include /** * Fixedwing attitude control app start / stop handling function diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f441c4a915..f8399d10be 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -90,7 +91,7 @@ #include #include "landingslope.h" #include "mtecs/mTecs.h" -#include +#include static int _control_task = -1; /**< task handle for sensor task */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe7..cb55a25aa7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp new file mode 100644 index 0000000000..f6b3268b59 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -0,0 +1,311 @@ +/* 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 mc_att_control_base.cpp + * + * MC Attitude Controller + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + * + */ + +#include "mc_att_control_base.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _publish_att_sp(false) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + _publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled + || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.z; + _publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi( + _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + + if (yaw_offs < -yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + } + + _v_att_sp.R_valid = false; + _publish_att_sp = true; + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + _publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x + * _params.man_pitch_max; + _v_att_sp.R_valid = false; + _publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp.R_body[0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, + _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} + +void MulticopterAttitudeControlBase::set_actuator_controls() +{ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h new file mode 100644 index 0000000000..f81a0d7721 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -0,0 +1,127 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_base.h + * + * MC Attitude Controller + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + * + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void set_actuator_controls(); + +protected: + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + } _params; + + bool _publish_att_sp; + + virtual void vehicle_attitude_setpoint_poll() = 0; + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 81a13edb8f..1103191e7f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. @@ -49,8 +52,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include +#include #include #include #include @@ -59,15 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include @@ -76,18 +69,56 @@ #include #include +#include "mc_att_control_base.h" + +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ + +using namespace px4; + /** * Multicopter attitude control app start / stop handling function * * @ingroup apps */ -extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(mc_att_control); + +int mc_attitude_thread_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControl +void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); +} + + +namespace px4 +{ +bool task_should_exit = false; +} + +// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener"); + + // px4::NodeHandle n; + + // PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000); + + /** + * px4::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). px4::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + // n.spin(); + // PX4_INFO("finished, returning"); + + // return 0; +// } + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase { public: /** @@ -100,17 +131,15 @@ public: */ ~MulticopterAttitudeControl(); - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); + void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + + void spin() { n.spin(); } private: + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -124,27 +153,7 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator controls */ - struct actuator_armed_s _armed; /**< actuator arming status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + px4::NodeHandle n; struct { param_t roll_p; @@ -170,19 +179,7 @@ private: param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - } _params; + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Update our local parameter cache. @@ -219,25 +216,6 @@ private: */ void arming_status_poll(); - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main attitude control task. - */ - void task_main(); }; namespace mc_att_control @@ -249,15 +227,14 @@ namespace mc_att_control #endif static const int ERROR = -1; -MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : - + MulticopterAttitudeControlBase(), _task_should_exit(false), _control_task(-1), - -/* subscriptions */ + _actuators_0_circuit_breaker_enabled(false), + /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -265,44 +242,16 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + n(), - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); @@ -326,6 +275,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + + /* + * do subscriptions + */ + // _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + // _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0); + // _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0); + // _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + PX4_SUBSCRIBE(n, vehicle_control_mode, 0); + // _params_sub = orb_subscribe(ORB_ID(parameter_update)); + PX4_SUBSCRIBE(n, parameter_update, 0); + // _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + PX4_SUBSCRIBE(n, manual_control_setpoint, 0); + // _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + PX4_SUBSCRIBE(n, actuator_armed, 0); + } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -349,7 +317,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - mc_att_control::g_control = nullptr; + // mc_att_control::g_control = nullptr; } int @@ -489,445 +457,213 @@ MulticopterAttitudeControl::arming_status_poll() } } -/* - * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) - */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; +// void +// MulticopterAttitudeControl::task_main() +// { - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); - } + // [> wakeup source: vehicle attitude <] + // struct pollfd fds[1]; - if (!_v_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; - } + // fds[0].fd = _v_att_sub; + // fds[0].events = POLLIN; - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } + // while (!_task_should_exit) { - /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); - } - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + // perf_end(_loop_perf); + // } - /* reset yaw setpint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _v_att_sp.yaw_body = _v_att.yaw; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + // warnx("exit"); - if (!_v_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + // _control_task = -1; + // _exit(0); +// } - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; } - _thrust_sp = _v_att_sp.thrust; + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); - if (_v_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + /* publish the attitude setpoint if needed */ + if (_publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; - } - - /* publish the attitude setpoint if needed */ - if (publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed.armed) { - _rates_int.zero(); - } - - /* current body angular rates */ - math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; - - /* angular rates error */ - math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } -} - -void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - mc_att_control::g_control->task_main(); -} - -void -MulticopterAttitudeControl::task_main() -{ - - /* - * do subscriptions - */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - /* initialize parameters cache */ - parameters_update(); - - /* wakeup source: vehicle attitude */ - struct pollfd fds[1]; - - fds[0].fd = _v_att_sub; - fds[0].events = POLLIN; - - while (!_task_should_exit) { - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) - continue; - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - perf_begin(_loop_perf); - - /* run controller on attitude changes */ - if (fds[0].revents & POLLIN) { - static uint64_t last_run = 0; - float dt = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too small (< 2ms) and too large (> 20ms) dt's */ - if (dt < 0.002f) { - dt = 0.002f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - - /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); - - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, + &_v_att_sp); } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { - /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; - - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; - } - } - - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); - - /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = hrt_absolute_time(); - - if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } - } + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), + &_v_att_sp); } } - perf_end(_loop_perf); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp.z; + + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; + } } - warnx("exit"); + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); - _control_task = -1; - _exit(0); -} + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); -int -MulticopterAttitudeControl::start() -{ - ASSERT(_control_task == -1); + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } } - - return OK; } -int mc_att_control_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(mc_att_control) { - if (argc < 1) + px4::init(argc, argv, "mc_att_control"); + + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) - errx(1, "already running"); - - mc_att_control::g_control = new MulticopterAttitudeControl; - - if (mc_att_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != mc_att_control::g_control->start()) { - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - err(1, "start failed"); + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); } + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + mc_attitude_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); } - if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) - errx(1, "not running"); + // if (!strcmp(argv[1], "stop")) { + // if (mc_att_control::g_control == nullptr) { + // errx(1, "not running"); + // } - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - exit(0); - } + // delete mc_att_control::g_control; + // mc_att_control::g_control = nullptr; + // exit(0); + // } - if (!strcmp(argv[1], "status")) { - if (mc_att_control::g_control) { - errx(0, "running"); + // if (!strcmp(argv[1], "status")) { + // if (mc_att_control::g_control) { + // errx(0, "running"); - } else { - errx(1, "not running"); - } - } + // } else { + // errx(1, "not running"); + // } + // } warnx("unrecognized command"); return 1; } + +int mc_attitude_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + MulticopterAttitudeControl attctl; + + thread_running = true; + + attctl.spin(); + + // while (!task_should_exit) { + // attctl.update(); + // } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + + diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 0000000000..d516d7e528 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,142 @@ +/* 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 mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; + + // setup rotation matrix + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 0000000000..a1bf44fc96 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,97 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69b..ecd251d45c 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control_base.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6682a9c89c..4ea5fdfb64 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin */ -#include -#include +#include +#include +#include #include #include #include @@ -54,8 +55,7 @@ #include #include #include -#include -#include + #include #include #include @@ -67,13 +67,13 @@ #include #include #include -#include -#include +// #include +// #include #include #include #include #include -#include +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +987,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2a601b6306..de6357d31b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 672dc52c3d..fca72c3040 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -187,8 +187,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + uint8_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -1512,7 +1512,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value) } } -switch_pos_t +uint8_t Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { @@ -1533,7 +1533,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi } } -switch_pos_t +uint8_t Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 3728f20672..6e22a557ea 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -41,6 +41,7 @@ #define SYSTEMLIB_H_ #include #include +#include __BEGIN_DECLS diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 61609d0092..fa0594c2ef 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -95,5 +95,7 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index d2ee754cdf..676c37c778 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,7 +40,7 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ -#include "../uORB.h" +#include #include /** diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index 6f16c51cf8..a61f078ba1 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -42,7 +42,7 @@ #include #include -#include "../uORB.h" +#include /** * @addtogroup topics diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h deleted file mode 100644 index 50b7bd9e59..0000000000 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 manual_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ -#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ - -#include -#include "../uORB.h" - -/** - * Switch position - */ -typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ - SWITCH_POS_ON, /**< switch activated (value = 1) */ - SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ - SWITCH_POS_OFF /**< switch not activated (value = -1) */ -} switch_pos_t; - -/** - * @addtogroup topics - * @{ - */ - -struct manual_control_setpoint_s { - uint64_t 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) - */ - float 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 */ - float 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 */ - float 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 */ - float r; /**< yaw stick/twist positon, -1..1 - in general corresponds to the righthand rotation around the vertical - (downwards) axis of the vehicle */ - float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(manual_control_setpoint); - -#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h deleted file mode 100644 index 8446e9c6e2..0000000000 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 vehicle_attitude_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * vehicle attitude setpoint. - */ -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< 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 - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float 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) */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h deleted file mode 100644 index ca77054569..0000000000 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. - * - * All control apps should depend their actions based on the flags set here. - * - * @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include -#include -#include "../uORB.h" -#include "vehicle_status.h" - -/** - * @addtogroup topics @{ - */ - - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ - -struct vehicle_control_mode_s { - uint64_t 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 */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b8936..f7a4324950 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,7 +45,7 @@ #include #include -#include "../uORB.h" +#include /** * @addtogroup topics diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 31e616f4f7..7888a50f4c 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -79,7 +79,7 @@ struct vehicle_gps_position_s { bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ uint8_t satellites_used; /**< Number of satellites used */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 8ec9d98d61..131029061d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -53,7 +53,7 @@ #include #include -#include "../uORB.h" +#include /** * @addtogroup topics @{ diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad26..578ebf9ddc 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -125,7 +125,7 @@ typedef intptr_t orb_advert_t; * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -261,4 +261,11 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; __END_DECLS +/* Diverse uORB header defines */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); + #endif /* _UORB_UORB_H */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8147a8b898..60901e0c75 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include @@ -612,10 +614,32 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { printf("%d ", (int)(_outputs.output[i]*1000)); } printf("\n"); + + // ESC status + int esc_sub = orb_subscribe(ORB_ID(esc_status)); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), esc_sub, &esc); + + printf("ESC Status:\n"); + printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d\t", esc.esc[i].esc_address); + printf("%3.2f\t", (double)esc.esc[i].esc_voltage); + printf("%3.2f\t", (double)esc.esc[i].esc_current); + printf("%3.2f\t", (double)esc.esc[i].esc_temperature); + printf("%3.2f\t", (double)esc.esc[i].esc_setpoint); + printf("%d\t", esc.esc[i].esc_rpm); + printf("%d", esc.esc[i].esc_errorcount); + printf("\n"); + } + + orb_unsubscribe(esc_sub); } // Sensor bridges diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 0000000000..9bb190380e --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * 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_defines.h + * + * Generally used magic defines + */ + +#pragma once +/* Get the name of the default value fiven the param name */ +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT + +/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */ +#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +#include "ros/ros.h" +/* Main entry point */ +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) + +/* Print/output wrappers */ +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO + +/* Topic Handle */ +#define PX4_TOPIC(_name) #_name + +/* Topic type */ +#define PX4_TOPIC_T(_name) _name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); + +/* Parameter handle datatype */ +typedef const char *px4_param_t; + +/* Helper fucntions to set ROS params, only int and float supported */ +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; + +/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + +#else +/* + * Building for NuttX + */ +#include +#ifdef __cplusplus +#include +#endif +/* Main entry point */ +#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) + +/* Print/output wrappers */ +#define PX4_WARN warnx +#define PX4_INFO warnx + +/* Topic Handle */ +#define PX4_TOPIC(_name) ORB_ID(_name) + +/* Topic type */ +#define PX4_TOPIC_T(_name) _name##_s + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval) +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) + +/* Parameter handle datatype */ +#include +typedef param_t px4_param_t; + +/* Initialize a param, get param handle */ +#define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) + +/* XXX this is a hack to resolve conflicts with NuttX headers */ +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') + +#endif + +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all + */ +#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) + +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER(_name) Subscriber + +/* shortcut for advertising topics */ +#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) + +/* wrapper for 2d matrices */ +#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) + +/* wrapper for rotation matrices stored in arrays */ +#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/platforms/px4_includes.h similarity index 61% rename from src/modules/uORB/topics/actuator_controls.h rename to src/platforms/px4_includes.h index e768ab2f61..525f94aae9 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/platforms/px4_includes.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,47 +32,39 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file px4_includes.h * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller + * Includes headers depending on the build target */ -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H +#pragma once -#include -#include "../uORB.h" +#include -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** - * @addtogroup topics - * @{ +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment */ +#include "ros/ros.h" +#include "px4/rc_channels.h" -struct actuator_controls_s { - uint64_t timestamp; - float control[NUM_ACTUATOR_CONTROLS]; -}; - -/** - * @} +#else +/* + * Building for NuttX */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index dece729070..16e73ec04a 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -50,18 +50,31 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -bool ok() { return ros::ok(); } +/** + * Returns true if the app/task should continue to run + */ +inline bool ok() { return ros::ok(); } #else extern bool task_should_exit; -bool ok() { return !task_should_exit; } +/** + * Returns true if the app/task should continue to run + */ +__EXPORT inline bool ok() { return !task_should_exit; } #endif class Rate { public: + /** + * Construct the Rate object and set rate + * @param rate_hz rate from which sleep time is calculated in Hz + */ explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + /** + * Sleep for 1/rate_hz s + */ void sleep() { usleep(sleep_interval); } private: diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4153517565..9bd792c697 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -55,7 +55,6 @@ namespace px4 { -//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -67,44 +66,85 @@ public: _pubs() {} - ~NodeHandle() { + ~NodeHandle() + { //XXX empty lists }; - /* Constructor with callback to function */ + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template - Subscriber * subscribe(const char *topic, void(*fp)(M)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) + { + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; - } - /* Constructor with callback to class method */ - template - Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber * sub = new Subscriber(ros_sub); - _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } + /** + * Subscribe with callback to class method + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ + template + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + { + SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } + + /** + * Subscribe with no callback, just the latest value is stored on updates + * @param topic Name of the topic + */ template - Publisher * advertise(const char *topic) { + Subscriber *subscribe(const char *topic) + { + SubscriberBase *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } + + /** + * Advertise topic + * @param topic Name of the topic + */ + template + Publisher *advertise(const char *topic) + { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; } + /** + * Calls all callback waiting to be called + */ + void spinOnce() { ros::spinOnce(); } + + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { ros::spin(); } - void spinOnce() { ros::spinOnce(); } + private: - static const uint32_t kQueueSizeDefault = 1000; - std::list _subs; - std::list _pubs; + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; -#else +#else //Building for NuttX class __EXPORT NodeHandle { public: @@ -116,27 +156,45 @@ public: ~NodeHandle() {}; + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + template - Subscriber * subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) { - SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); + Subscriber *subscribe(const struct orb_metadata *meta, + std::function callback, + unsigned interval) + { + SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { _sub_min_interval = sub_px4; } - return (Subscriber*)sub_px4; + + return (Subscriber *)sub_px4; } + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ template - Publisher * advertise(const struct orb_metadata *meta) { + Publisher *advertise(const struct orb_metadata *meta) + { //XXX - Publisher * pub = new Publisher(meta, &_pubs); + Publisher *pub = new Publisher(meta, &_pubs); return pub; } - void spinOnce() { + /** + * Calls all callback waiting to be called + */ + void spinOnce() + { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; @@ -152,9 +210,14 @@ public: } } - void spin() { + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ + void spin() + { while (ok()) { const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ if (_sub_min_interval == nullptr) { usleep(timeout_ms * 1000); @@ -165,6 +228,7 @@ public: struct pollfd pfd; pfd.fd = _sub_min_interval->getHandle(); pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { /* timed out */ continue; @@ -175,9 +239,9 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; - List _pubs; - uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ + uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 9ce211d250..6b6d8e39e9 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -48,37 +48,75 @@ namespace px4 { -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class Publisher + +/** + * Untemplated publisher base class + * */ +class PublisherBase { public: - Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) + PublisherBase() {}; + ~PublisherBase() {}; + +}; + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class Publisher : + public PublisherBase +{ +public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ + Publisher(ros::Publisher ros_pub) : + PublisherBase(), + _ros_pub(ros_pub) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template int publish(const M &msg) { _ros_pub.publish(msg); return 0; } private: - ros::Publisher _ros_pub; + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : - public uORB::PublicationNode + public uORB::PublicationNode, + public PublisherBase { public: + /** + * Construct Publisher by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param list publisher is added to this list + */ Publisher(const struct orb_metadata *meta, - List * list) : - uORB::PublicationNode(meta, list) + List *list) : + uORB::PublicationNode(meta, list), + PublisherBase() {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template - int publish(const M &msg) { - uORB::PublicationBase::update((void*)&msg); + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); return 0; } - void update() { - //XXX list traversal callback, needed? - } ; + /** + * Empty callback for list traversal + */ + void update() {} ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6e0ef8aed6..59c0ef70bd 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,6 +36,7 @@ * * PX4 Middleware Wrapper Subscriber */ +#include #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -44,51 +45,148 @@ /* includes when building for NuttX */ #include #include -#include +#include "px4_nodehandle.h" #endif namespace px4 { -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class Subscriber +/** + * Untemplated subscriber base class + * */ +class SubscriberBase { public: - Subscriber(ros::Subscriber ros_sub) : - _ros_sub(ros_sub) - {} - ~Subscriber() {}; -private: - ros::Subscriber _ros_sub; -}; -#else -// typedef std::function CallbackFunction; -class Subscriber -{ -public: - Subscriber() {}; - ~Subscriber() {}; -private: + SubscriberBase() {}; + ~SubscriberBase() {}; + }; +/** + * Subscriber class which is used by nodehandle + */ template -class SubscriberPX4 : - public Subscriber, +class Subscriber : + public SubscriberBase +{ +public: + Subscriber() : + SubscriberBase() + {}; + ~Subscriber() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + virtual const M& get_msg() = 0; +}; + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Subscriber class that is templated with the ros n message type + */ +template +class SubscriberROS : + public Subscriber +{ +friend class NodeHandle; + +public: + /** + * Construct Subscriber by providing a callback function + */ + SubscriberROS(std::function cbf) : + Subscriber(), + _ros_sub(), + _cbf(cbf), + _msg_current() + {} + + /** + * Construct Subscriber without a callback function + */ + SubscriberROS() : + Subscriber(), + _ros_sub(), + _cbf(NULL), + _msg_current() + {} + + + ~SubscriberROS() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return _msg_current; } + +protected: + /** + * Called on topic update, saves the current message and then calls the provided callback function + */ + void callback(const M &msg) { + /* Store data */ + _msg_current = msg; + + /* Call callback */ + if (_cbf != NULL) { + _cbf(msg); + } + + } + + /** + * Saves the ros subscriber to keep ros subscription alive + */ + void set_ros_sub(ros::Subscriber ros_sub) { + _ros_sub = ros_sub; + } + + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ + std::function _cbf; /**< Callback that the user provided on the subscription */ + M _msg_current; /**< Current Message value */ + +}; + +#else // Building for NuttX + +/** + * Subscriber class that is templated with the uorb subscription message type + */ +template +class SubscriberUORB : + public Subscriber, public uORB::Subscription { public: - SubscriberPX4(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List * list) : - Subscriber(), + /** + * Construct SubscriberUORB by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ + SubscriberUORB(const struct orb_metadata *meta, + unsigned interval, + std::function callback, + List *list) : + Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) //XXX store callback {} - ~SubscriberPX4() {}; - void update() { + ~SubscriberUORB() {}; + + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ + void update() + { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -97,12 +195,26 @@ public: /* get latest data */ uORB::Subscription::update(); + + /* Check if there is a callback */ + if (_callback == nullptr) { + return; + } + /* Call callback which performs actions based on this data */ _callback(uORB::Subscription::getData()); }; -private: - std::function _callback; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return uORB::Subscription::getData(); } + +protected: + std::function _callback; /**< Callback handle, + called when new data is available */ }; #endif diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp new file mode 100755 index 0000000000..aa8d85c60c --- /dev/null +++ b/src/platforms/ros/perf_counter.cpp @@ -0,0 +1,142 @@ +/* + * perf_counter.c + + * + * Created on: Sep 24, 2014 + * Author: roman + */ +#include +#include +#include + + + +perf_counter_t perf_alloc(enum perf_counter_type type, const char *name) +{ + return NULL; +} + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +void perf_free(perf_counter_t handle) +{ + +} + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_count(perf_counter_t handle) +{ + +} + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_begin(perf_counter_t handle) +{ + +} + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_end(perf_counter_t handle) +{ + +} + +/** + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_cancel(perf_counter_t handle) +{ + +} + +/** + * Reset a performance counter. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +void perf_reset(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to stdout + * + * @param handle The counter to print. + */ +void perf_print_counter(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +void perf_print_counter_fd(int fd, perf_counter_t handle) +{ + +} + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +void perf_print_all(int fd) +{ + +} + +/** + * Reset all of the performance counters. + */ +void perf_reset_all(void) +{ + +} + +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +uint64_t perf_event_count(perf_counter_t handle) +{ + +} + + diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 7d80af3079..20967b541d 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,6 +39,7 @@ */ #include +#include #include #include