diff options
105 files changed, 1147 insertions, 813 deletions
diff --git a/.gitmodules b/.gitmodules index c869803b7..6a4f815d4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [submodule "unittests/gtest"] path = unittests/gtest url = https://github.com/sjwilks/gtest.git +[submodule "src/lib/eigen"] + path = src/lib/eigen + url = https://github.com/PX4/eigen.git diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 index c2f94705c..a36bac5cc 100644 --- a/ROMFS/px4fmu_common/init.d/50001_rover +++ b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 @@ -1,10 +1,10 @@ #!nsh # -# Generic rover +# loading default values for the axialracing ax10 # #load some defaults e.g. PWM values -sh /etc/init.d/rc.rover_defaults +sh /etc/init.d/rc.axialracing_ax10_defaults #choose a mixer, for rover control we need a plain passthrough to the servos set MIXER IO_pass diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 434e7870b..de81795b4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -287,10 +287,10 @@ fi # -# Ground Rover +# Ground Rover AxialRacing AX10 # if param compare SYS_AUTOSTART 50001 then - sh /etc/init.d/50001_rover + sh /etc/init.d/50001_axialracing_ax10 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps index 1d15b9835..625febd7a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps @@ -6,4 +6,5 @@ ekf_att_pos_estimator start -rover_steering_control start +# disabled the start of steering control app due to use of offboard mode only +# rover_steering_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults index 23bf47df1..e5d400abe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults @@ -19,8 +19,8 @@ set PWM_RATE 50 set PWM_DISARMED 1500 # PWM range -set PWM_MIN 1200 -set PWM_MAX 1800 +set PWM_MIN 1100 +set PWM_MAX 1900 # Enable servo output on pins 3 and 4 (steering and thrust) # but also include 1+2 as they form together one output group diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 59abdc8e3..111965bd5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -631,7 +631,7 @@ then # Start standard rover apps if [ $LOAD_DAPPS == yes ] then - sh /etc/init.d/rc.rover_apps + sh /etc/init.d/rc.axialracing_ax10_apps fi fi diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 8d1ab6363..2353a5232 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -3,7 +3,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 5e6e57164..4b251642c 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -74,6 +74,28 @@ else git submodule update; fi +if [ -d src/lib/eigen ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked Eigen submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "eigen sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + if [ -d Tools/gencpp ] then STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 7efc400cd..b65df8dce 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -27,7 +27,7 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc sudo apt-get -y install python-rosinstall # additional dependencies -sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy +sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy ros-indigo-mavros ros-indigo-mavros-extras ## drcsim setup (for models) ### add osrf repository diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 3e4bf06a5..5599ab999 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -10,21 +10,19 @@ git clone https://github.com/PX4/Firmware.git # euroc simulator git clone https://github.com/PX4/euroc_simulator.git -cd euroc_simulator -git checkout px4_nodes -cd .. # mav comm git clone https://github.com/PX4/mav_comm.git +# gflags catkin +git clone https://github.com/ethz-asl/gflags_catkin.git + # glog catkin git clone https://github.com/ethz-asl/glog_catkin.git # catkin simple git clone https://github.com/catkin/catkin_simple.git -# drcsim (for scenery and models) - cd .. catkin_make diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index d61eec063..734bcf590 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -39,6 +39,7 @@ PKG = 'px4' import unittest import rospy +import rosbag from numpy import linalg import numpy as np @@ -47,9 +48,11 @@ from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint +from px4.msg import vehicle_local_position_setpoint from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by directly sending setpoints @@ -62,8 +65,11 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.helper = PX4TestHelper("direct_offboard_posctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False @@ -74,9 +80,12 @@ class DirectOffboardPosctlTest(unittest.TestCase): if self.fpa: self.fpa.stop() + self.helper.tearDown() + # # General callback functions used in tests # + def position_callback(self, data): self.has_pos = True self.local_position = data @@ -105,6 +114,7 @@ class DirectOffboardPosctlTest(unittest.TestCase): stp = position_setpoint_triplet() stp.current = pos self.pub_spt.publish(stp) + self.helper.bag_write('px4/position_setpoint_triplet', stp) # does it reach the position in X seconds? count = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 30e9fe9ba..ef875ce61 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -39,12 +39,17 @@ PKG = 'px4' import unittest import rospy +import rosbag from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending attitude and thrust setpoints @@ -57,6 +62,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) rospy.wait_for_service('iris/mavros/cmd/arming', 30) + self.helper = PX4TestHelper("mavros_offboard_attctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) @@ -66,6 +74,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.control_mode = vehicle_control_mode() self.local_position = PoseStamped() + def tearDown(self): + self.helper.tearDown() + # # General callback functions used in tests # @@ -99,7 +110,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pub_att.publish(att) + self.helper.bag_write('mavros/setpoint/attitude', att) self.pub_thr.publish(throttle) + self.helper.bag_write('mavros/setpoint/att_throttle', throttle) self.rate.sleep() if (self.local_position.pose.position.x > 5 diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 568c2cbd8..1383cd04c 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -40,14 +40,18 @@ PKG = 'px4' import unittest import rospy import math +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_local_position_setpoint from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending position setpoints @@ -60,6 +64,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.helper = PX4TestHelper("mavros_offboard_posctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) @@ -68,6 +75,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.local_position = PoseStamped() self.control_mode = vehicle_control_mode() + def tearDown(self): + self.helper.tearDown() + # # General callback functions used in tests # @@ -118,6 +128,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pub_spt.publish(pos) + self.helper.bag_write('mavros/setpoint/local_position', pos) if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/demo_tests/px4_test_helper.py new file mode 100644 index 000000000..4dc886634 --- /dev/null +++ b/integrationtests/demo_tests/px4_test_helper.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import unittest +import rospy +import rosbag + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude +from px4.msg import vehicle_local_position_setpoint + +from threading import Condition + +# +# Test helper +# +class PX4TestHelper(object): + + def __init__(self, test_name): + self.test_name = test_name + + def setUp(self): + self.condition = Condition() + self.closed = False + + rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4") + + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", + vehicle_local_position, self.vehicle_position_callback) + self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint", + vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback) + self.sub_va = rospy.Subscriber("iris/vehicle_attitude", + vehicle_attitude, self.vehicle_attitude_callback) + self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint", + vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback) + + + def tearDown(self): + try: + self.condition.acquire() + self.closed = True + + self.sub_vlp.unregister() + self.sub_vasp.unregister() + self.sub_va.unregister() + self.sub_vlps.unregister() + self.bag.close() + + finally: + self.condition.release() + + def vehicle_position_callback(self, data): + self.bag_write('px4/vehicle_local_position', data) + + def vehicle_attitude_setpoint_callback(self, data): + self.bag_write('px4/vehicle_attitude_setpoint', data) + + def vehicle_attitude_callback(self, data): + self.bag_write('px4/vehicle_attitude', data) + + def vehicle_local_position_setpoint_callback(self, data): + self.bag_write('px4/vehicle_local_position_setpoint', data) + + def bag_write(self, topic, data): + try: + self.condition.acquire() + if not self.closed: + self.bag.write(topic, data) + else: + rospy.logwarn("Trying to write to bag but it's already closed") + finally: + self.condition.release() + diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 0d3934bd0..9ab63a973 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -27,7 +27,7 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/boardinfo +MODULES += systemcmds/ver MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f414186d1..324e12696 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -50,7 +50,6 @@ MODULES += drivers/gimbal # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 29eb68096..568f940ee 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -48,7 +48,6 @@ MODULES += drivers/px4flow # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 22563838b..0d04a1f19 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,7 +42,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 2c5302812..3b9fefb3e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -296,6 +296,9 @@ endef # # - compile an empty file to generate a suitable object file # - relink the object and insert the binary file +# - extract the length +# - create const unsigned $3_len with the extracted length as its value and compile it to an object file +# - link the two generated object files together # - edit symbol names to suit # # NOTE: exercise caution using this with absolute pathnames; it looks @@ -320,11 +323,14 @@ define BIN_TO_OBJ @$(MKDIR) -p $(dir $2) $(Q) $(ECHO) > $2.c $(call COMPILE,$2.c,$2.c.o) - $(Q) $(LD) -r -o $2 $2.c.o -b binary $1 + $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 + $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c + $(call COMPILE,$2.c,$2.c.o) + $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o $(Q) $(OBJCOPY) $2 \ --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \ - --redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \ + --strip-symbol $(call BIN_SYM_PREFIX,$1)_size \ --strip-symbol $(call BIN_SYM_PREFIX,$1)_end \ --rename-section .data=.rodata - $(Q) $(REMOVE) $2.c $2.c.o + $(Q) $(REMOVE) $2.c $2.c.o $2.bin.o endef diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h index 8705c1bc2..6dc85735a 100644 --- a/nuttx-configs/aerocore/include/board.h +++ b/nuttx-configs/aerocore/include/board.h @@ -145,7 +145,7 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ @@ -203,7 +203,7 @@ /* * SPI */ -/* PA[4-7] SPI1 broken out on J12 */ +/* PA[4-7] SPI1 broken out on J12 */ #define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/aerocore/src/empty.c +++ b/nuttx-configs/aerocore/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index ff1c63424..9e6d50a8b 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -209,7 +209,7 @@ /* UART DMA configuration for USART1/6 */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - + /* * CAN * diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4fmu-v1/src/empty.c +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3b3c6fa70..52668cacd 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -146,21 +146,21 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ #define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) #define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) -/* SDIO dividers. Note that slower clocking is required when DMA is disabled +/* SDIO dividers. Note that slower clocking is required when DMA is disabled * in order to avoid RX overrun/TX underrun errors due to delayed responses * to service FIFOs in interrupt driven mode. These values have not been * tuned!!! * * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz */ - + #define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz @@ -168,9 +168,9 @@ */ #ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) #else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz @@ -227,10 +227,10 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* +/* * CAN * - * CAN1 is routed to the onboard transceiver. + * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ #define GPIO_CAN1_RX GPIO_CAN1_RX_3 diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4fmu-v2/src/empty.c +++ b/nuttx-configs/px4fmu-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h index 815079266..ee8a9d1ba 100755 --- a/nuttx-configs/px4io-v1/include/board.h +++ b/nuttx-configs/px4io-v1/include/board.h @@ -143,7 +143,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4io-v1/src/empty.c +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 17e77918b..b70056f82 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -138,7 +138,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4io-v2/src/empty.c +++ b/nuttx-configs/px4io-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index a958fc60d..9f72bd56f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,6 +84,7 @@ #define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -179,6 +180,7 @@ private: orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery + uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -197,7 +199,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _reports(nullptr), _batt_topic(-1), _batt_orb_id(nullptr), - _start_time(0) + _start_time(0), + _batt_design_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -263,7 +266,7 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); } } @@ -279,6 +282,7 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; + int16_t orig_addr = get_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { @@ -293,6 +297,9 @@ BATT_SMBUS::search() usleep(1); } + // restore original i2c address + set_address(orig_addr); + // display completion message if (found_slave) { warnx("Done."); @@ -364,13 +371,28 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - usleep(1); uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; } + // read battery design capacity + if (_batt_design_capacity == 0) { + if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { + _batt_design_capacity = tmp; + } + } + + // read remaining capacity + if (_batt_design_capacity > 0) { + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (tmp < _batt_design_capacity) { + new_report.discharged_mah = _batt_design_capacity - tmp; + } + } + } + // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); @@ -430,8 +452,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ { uint8_t buff[max_len + 2]; // buffer to hold results - usleep(1); - // read bytes including PEC int ret = transfer(®, 1, buff, max_len + 2); diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 776a2071e..19518d73d 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include <arch/board/board.h> #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 882ec6aa2..17fa9c542 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -52,7 +52,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include <stm32.h> #include <arch/board/board.h> - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 273af1023..6188e29ae 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include <arch/board/board.h> #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 8292da9e1..5a02a9716 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -87,7 +87,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_RELAY1_EN); stm32_configgpio(GPIO_RELAY2_EN); - /* turn off - all leds are active low */ + /* turn off - all leds are active low */ stm32_gpiowrite(GPIO_LED1, true); stm32_gpiowrite(GPIO_LED2, true); stm32_gpiowrite(GPIO_LED3, true); diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 5c3343ccc..fd7eb33c3 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -126,7 +126,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ stm32_configgpio(GPIO_SBUS_OUTPUT); - + /* sbus output enable is active low - disable it by default */ stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 2ff91d5d0..fddef3626 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -35,7 +35,7 @@ * @file drv_airspeed.h * * Airspeed driver interface. - * + * * @author Simon Wilks */ diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 7258c9e84..8568436d3 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -60,7 +60,7 @@ /** play the numbered script in (arg), repeating forever */ #define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving <duration>, <command>, arg[0-2] * diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index e0b16fa5c..c1db6b534 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -36,7 +36,7 @@ /** * @file drv_orb_dev.h - * + * * uORB published object driver. */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 0dcb10a7b..00e368318 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -117,7 +117,7 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_ENUM_COUNT }; -/* +/* structure passed to OREOLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h index 778d9fcf5..47b84e6e1 100644 --- a/src/drivers/drv_pwm_input.h +++ b/src/drivers/drv_pwm_input.h @@ -46,6 +46,6 @@ __BEGIN_DECLS /* * Initialise the timer */ -__EXPORT extern int pwm_input_main(int argc, char * argv[]); +__EXPORT extern int pwm_input_main(int argc, char *argv[]); __END_DECLS diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 35e9619f0..6271ad208 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -78,7 +78,7 @@ __BEGIN_DECLS /** * Highest PWM allowed as the minimum PWM */ -#define PWM_HIGHEST_MIN 1300 +#define PWM_HIGHEST_MIN 1600 /** * Highest maximum PWM in us @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1700 +#define PWM_LOWEST_MAX 1400 /** * Do not output a channel with this value diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ccda4c0a5..1e27309d8 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -121,8 +121,12 @@ private: int _vehicle_command_sub; int _att_sub; - bool _attitude_compensation; + bool _attitude_compensation_roll; + bool _attitude_compensation_pitch; + bool _attitude_compensation_yaw; bool _initialized; + bool _control_cmd_set; + bool _config_cmd_set; orb_advert_t _actuator_controls_2_topic; @@ -130,6 +134,9 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + struct vehicle_command_s _control_cmd; + struct vehicle_command_s _config_cmd; + /** * Initialise the automatic measurement state machine and start it. * @@ -169,7 +176,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), - _attitude_compensation(true), + _attitude_compensation_roll(true), + _attitude_compensation_pitch(true), + _attitude_compensation_yaw(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), @@ -221,7 +230,9 @@ Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case GIMBALIOCATTCOMPENSATE: - _attitude_compensation = (arg != 0); + _attitude_compensation_roll = (arg != 0); + _attitude_compensation_pitch = (arg != 0); + _attitude_compensation_yaw = (arg != 0); return OK; default: @@ -286,22 +297,30 @@ Gimbal::cycle() float yaw = 0.0f; - if (_attitude_compensation) { - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - } + if (_att_sub < 0) { + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } - vehicle_attitude_s att; + vehicle_attitude_s att; - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + if (_attitude_compensation_roll) { roll = -att.roll; + updated = true; + } + + if (_attitude_compensation_pitch) { pitch = -att.pitch; - yaw = att.yaw; + updated = true; + } + if (_attitude_compensation_yaw) { + yaw = att.yaw; updated = true; } + struct vehicle_command_s cmd; bool cmd_updated; @@ -312,22 +331,47 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); - VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7; - debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2); + if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + + _control_cmd = cmd; + _control_cmd_set = true; + + } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + _config_cmd = cmd; + _config_cmd_set = true; + } + + } + + if (_config_cmd_set) { + + _config_cmd_set = false; + + _attitude_compensation_roll = (int)_config_cmd.param2 == 1; + _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; + _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; + + } + + if (_control_cmd_set) { + + VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ - roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1; - pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2; - yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3; + roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; + pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; + yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; updated = true; - } - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { - float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); roll += gimablDirectionEuler(0); @@ -336,6 +380,7 @@ Gimbal::cycle() updated = true; } + } if (updated) { diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index e91e91fc0..9607fe614 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,4 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; -typedef device::Device* (*HMC5883_constructor)(int); +typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c8211b8dd..c946e38cb 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,4 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6..1512566fa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1291,6 +1291,24 @@ PX4IO::io_set_rc_config() input_map[ichan - 1] = 4; } + /* AUX 1*/ + param_get(param_find("RC_MAP_AUX1"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 5; + } + + /* AUX 2*/ + param_get(param_find("RC_MAP_AUX2"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 6; + } + + /* AUX 3*/ + param_get(param_find("RC_MAP_AUX3"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 7; + } + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d8b777b91..17b27290c 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -112,8 +112,9 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators); +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators); /** * Control heading. @@ -128,7 +129,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att_sp The attitude setpoint. This is the output of the controller */ void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ static bool thread_should_exit = false; /**< Daemon exit flag */ @@ -137,8 +138,9 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators) +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators) { /* @@ -175,7 +177,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st } void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { /* @@ -192,6 +194,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { att_sp->roll_body = 0.6f; } @@ -285,7 +288,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN }}; + { .fd = att_sub, .events = POLLIN } + }; while (!thread_should_exit) { @@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (manual_sp_updated) /* get the RC (or otherwise user based) input */ + { orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ if (isfinite(manual_sp.z) && - (manual_sp.z >= 0.6f) && - (manual_sp.z <= 1.0f)) { + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ @@ -378,8 +384,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) thread_running = false; /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; + } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -393,8 +400,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); exit(1); @@ -410,8 +418,9 @@ usage(const char *reason) */ int ex_fixedwing_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index c2e94ff3d..fbbd30201 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -34,7 +34,7 @@ /* * @file params.c - * + * * Parameters for fixedwing demo */ diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 4ae8e90ec..49ffff446 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -34,7 +34,7 @@ /* * @file params.h - * + * * Definition of parameters for fixedwing example */ 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 da4ea825b..f8e3f2dc5 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -81,8 +81,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -97,13 +99,12 @@ static void usage(const char *reason) */ int flow_position_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { printf("flow position estimator already running\n"); /* this is not an error */ exit(0); @@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + flow_position_estimator_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { - if (thread_running) + if (!strcmp(argv[1], "status")) { + if (thread_running) { printf("\tflow position estimator is running\n"); - else + + } else { printf("\tflow position estimator not started\n"); + } exit(0); } @@ -147,9 +148,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* rotation matrix for transformation of optical flow speed vectors */ static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, - { 1, 0, 0 }, - { 0, 0, 1 }}; // 90deg rotated - const float time_scale = powf(10.0f,-6.0f); + { 1, 0, 0 }, + { 0, 0, 1 } + }; // 90deg rotated + const float time_scale = powf(10.0f, -6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; static float global_speed[3] = {0.0f, 0.0f, 0.0f}; @@ -192,18 +194,18 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* init local position and filtered flow struct */ struct vehicle_local_position_s local_pos = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - .vx = 0.0f, - .vy = 0.0f, - .vz = 0.0f + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f }; struct filtered_bottom_flow_s filtered_flow = { - .sumx = 0.0f, - .sumy = 0.0f, - .vx = 0.0f, - .vy = 0.0f + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f }; /* advert pub messages */ @@ -224,37 +226,29 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); - while (!thread_should_exit) - { + while (!thread_should_exit) { - if (sensors_ready) - { + if (sensors_ready) { /*This runs at the rate of the sensors */ struct pollfd fds[2] = { - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = parameter_update_sub, .events = POLLIN } + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, 2, 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ // printf("[flow position estimator] no bottom flow.\n"); - } - else - { + } else { /* parameter update available? */ - if (fds[1].revents & POLLIN) - { + if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -264,8 +258,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* only if flow data changed */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { perf_begin(mc_loop_perf); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); @@ -284,46 +277,48 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> minimum sonar value 0.3m */ - if (!vehicle_liftoff) - { - if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (!vehicle_liftoff) { + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) { vehicle_liftoff = true; - } - else - { - if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + } + + } else { + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) { vehicle_liftoff = false; + } } /* calc dt between flow timestamps */ /* ignore first flow msg */ - if(time_last_flow == 0) - { + if (time_last_flow == 0) { time_last_flow = flow.timestamp; continue; } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; time_last_flow = flow.timestamp; /* only make position update if vehicle is lift off or DEBUG is activated*/ - if (vehicle_liftoff || params.debug) - { + if (vehicle_liftoff || params.debug) { /* copy flow */ if (flow.integration_timespan > 0) { flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + } else { flow_speed[0] = 0; flow_speed[1] = 0; } + flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + } speed[i] = sum; } @@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* convert to globalframe velocity * -> local position is currently not used for position control */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + speed[j] * PX4_R(att.R, i, j); + } global_speed[i] = sum; } @@ -354,9 +350,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.vy = global_speed[1]; local_pos.xy_valid = true; local_pos.v_xy_valid = true; - } - else - { + + } else { /* set speed to zero and let position as it is */ filtered_flow.vx = 0; filtered_flow.vy = 0; @@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !armed.armed) - { + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; local_pos.z_valid = false; - } - else - { + + } else { sonar_valid = true; } - if (sonar_valid || params.debug) - { + if (sonar_valid || params.debug) { /* simple lowpass sonar filtering */ /* if new value or with sonar update frequency */ - if (sonar_new != sonar_last || counter % 10 == 0) - { + if (sonar_new != sonar_last || counter % 10 == 0) { sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; sonar_last = sonar_new; } @@ -392,12 +383,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) float height_diff = sonar_new - sonar_lp; /* if over 1/2m spike follow lowpass */ - if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) - { + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) { local_pos.z = -sonar_lp; - } - else - { + + } else { local_pos.z = -sonar_new; } @@ -408,15 +397,14 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.timestamp = hrt_absolute_time(); /* publish local position */ - if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) - && isfinite(local_pos.vx) && isfinite(local_pos.vy)) - { + if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) { orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } /* publish filtered flow */ - if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) - { + if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) + && isfinite(filtered_flow.vy)) { orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); } @@ -427,9 +415,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } } - } - else - { + } else { /* sensors not ready waiting for first attitude msg */ /* polling */ @@ -440,19 +426,16 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* wait for a attitude message, check for exit condition every 5 s */ int ret = poll(fds, 1, 5000); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + + } else if (ret == 0) { /* no return value, ignore */ printf("[flow position estimator] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN){ + + } else { + if (fds[0].revents & POLLIN) { sensors_ready = true; printf("[flow position estimator] initialized.\n"); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c index ec3c3352d..b56579787 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.c - * + * * Parameters for position estimator */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h index f9a9bb303..3ab4e560f 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.h - * + * * Parameters for position estimator */ diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 145cf99cc..8e439bdac 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,8 +73,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -89,13 +91,12 @@ static void usage(const char *reason) */ int matlab_csv_serial_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { warnx("already running\n"); /* this is not an error */ exit(0); @@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { + if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("stopped"); } @@ -139,7 +139,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) errx(1, "need a serial port name as argument"); } - const char* uart_name = argv[1]; + const char *uart_name = argv[1]; warnx("opening port %s", uart_name); @@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) thread_running = true; - while (!thread_should_exit) - { + while (!thread_should_exit) { /*This runs at the rate of the sensors */ struct pollfd fds[] = { - { .fd = accel0_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, ignore */ - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ warnx("no sensor data"); - } - else - { + + } else { /* accel0 update available? */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, + dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, + (int)accel0.z_raw, (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 8165b0ffc..0a66d3edc 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -40,7 +40,8 @@ #pragma once #include <px4.h> -class PublisherExample { +class PublisherExample +{ public: PublisherExample(); @@ -49,7 +50,7 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; - px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub; - px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub; + px4::Publisher<px4::px4_rc_channels> *_rc_channels_pub; + px4::Publisher<px4::px4_vehicle_attitude> *_v_att_pub; + px4::Publisher<px4::px4_parameter_update> *_parameter_update_pub; }; diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index db9d85269..405b3c987 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 45d541137..860b1af78 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -34,7 +34,7 @@ /** * @file px4_daemon_app.c * daemon application example for PX4 autopilot - * + * * @author Example User <mail@example.com> */ @@ -71,8 +71,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); } @@ -80,14 +82,15 @@ usage(const char *reason) * The daemon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ int px4_daemon_app_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\trunning\n"); + } else { warnx("\tnot started\n"); } + exit(0); } @@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[]) exit(1); } -int px4_daemon_thread_main(int argc, char *argv[]) { +int px4_daemon_thread_main(int argc, char *argv[]) +{ warnx("[daemon] starting\n"); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 4e9f099ed..e22c59fa2 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -75,30 +75,33 @@ int px4_simple_app_main(int argc, char *argv[]) for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); - + /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ printf("[px4_simple_app] Got no data within a second\n"); + } else if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ printf("[px4_simple_app] ERROR return value from poll(): %d\n" - , poll_ret); + , poll_ret); } + error_counter++; + } else { - + if (fds[0].revents & POLLIN) { /* obtained data for the first file descriptor */ struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); /* set att and publish this information for other apps */ att.roll = raw.accelerometer_m_s2[0]; @@ -106,6 +109,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.yaw = raw.accelerometer_m_s2[2]; orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } + /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 7ef914098..96b40b23f 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -155,7 +155,6 @@ int parameters_update(const struct param_handles *h, struct params *p) void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) { - /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing @@ -267,18 +266,27 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ struct pollfd fds[2]; + fds[0].fd = param_sub; + fds[0].events = POLLIN; + fds[1].fd = att_sub; + fds[1].events = POLLIN; while (!thread_should_exit) { @@ -371,6 +379,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { actuators.control[i] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -421,7 +430,7 @@ int rover_steering_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, rover_steering_control_thread_main, - (argv) ? (char *const *)&argv[2] : (char *const *)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 496e1dcd8..5a23dc80b 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,7 +43,8 @@ using namespace px4; -void rc_channels_callback_function(const px4_rc_channels &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg) +{ PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -81,21 +82,24 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) +{ PX4_INFO("rc_channels_callback (method): [%llu]", - msg.data().timestamp_last_valid); + msg.data().timestamp_last_valid); PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } -void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) +{ PX4_INFO("vehicle_attitude_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); } -void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) +{ PX4_INFO("parameter_update_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); _p_test_float.update(); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 686fed023..9eb5dd2a0 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -43,7 +43,8 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg); -class SubscriberExample { +class SubscriberExample +{ public: SubscriberExample(); @@ -54,7 +55,7 @@ protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; - px4::Subscriber<px4_rc_channels> * _sub_rc_chan; + px4::Subscriber<px4_rc_channels> *_sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 6129b19ac..b450230c1 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 917c7f830..13fe701df 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,7 +118,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/eigen b/src/lib/eigen new file mode 160000 +Subproject e7850ed81f9c469e02df496ef09ae32ec0379b7 diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 4215b49d2..3b276c556 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -76,7 +76,7 @@ private: method is checked for further adavancing in the state machine (e.g. when to power up the motors) */ - LaunchMethod* launchMethods[1]; + LaunchMethod *launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h new file mode 100644 index 000000000..2cd98e59a --- /dev/null +++ b/src/lib/px4_eigen.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_eigen.h + * + * Compatability header to make Eigen compile on the PX4 stack + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#pragma once + +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include <eigen/Eigen/Core> +#include <eigen/Eigen/Geometry> +#pragma GCC diagnostic pop diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 454234601..0cf732824 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -158,6 +158,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ __EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS 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 5637ec102..e143f37b9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -177,6 +177,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru 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/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3c8e49ee9..ba4ca07cc 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -57,4 +57,5 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius);
\ No newline at end of file diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1b0c4258b..94eeb6f71 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * Defines the voltage where a single cell of the battery is considered empty. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -64,6 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * Defines the voltage where a single cell of the battery is considered full. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -74,6 +76,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * to maximum current ratio and assumes linearity. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -83,6 +86,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * Defines the number of cells the attached battery consists of. * * @group Battery Calibration + * @unit S */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -92,6 +96,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * Defines the capacity of the attached battery. * * @group Battery Calibration + * @unit mA */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index db30af416..aa82a4f59 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // rate of climb -// this is what rate of climb is commanded (in m/s) +// this is what rate of climb is commanded (in m/s) // when the pitch stick is fully defelcted in simple mode PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5b9e45d3e..57a92c8b5 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -80,6 +80,7 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); + if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9d647cb68..37a41b68a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -76,7 +76,7 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index a36a4688d..c70cbeb0e 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,6 +15,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) if (!isfinite(acc)) { acc = 0.0f; } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; x[1] += acc * dt; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f0..9d2849090 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -142,7 +142,7 @@ sbus1_output(uint16_t *values, uint16_t num_values) value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); /*protect from out of bounds values and limit to 11 bits*/ - if (value > 0x07ff ) { + if (value > 0x07ff) { value = 0x07ff; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7017a65ad..04bf71c17 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1216,12 +1216,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (status_updated) { log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; + log_msg.body.log_STAT.main_state = buf_status.main_state; + log_msg.body.log_STAT.arming_state = buf_status.arming_state; + log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; + log_msg.body.log_STAT.load = buf_status.load; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87b4795bc..54b0fe73f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,10 +171,11 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; - uint8_t failsafe_state; + uint8_t failsafe; float battery_remaining; uint8_t battery_warning; uint8_t landed; + float load; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -480,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 272e4b14f..1acc14fc6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -682,7 +682,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * * @group Sensor Calibration */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation X (Roll) offset diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ea478a60f..f5ff0afd4 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -45,7 +45,7 @@ #include <px4.h> #include <systemlib/circuit_breaker.h> -bool circuit_breaker_enabled(const char* breaker, int32_t magic) +bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c97dbc26f..c76e6c37f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b354eb518..b753da5ce 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -244,14 +244,23 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); + max_out = 0.0f; + /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + + /* update max output value */ + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; } _limits.roll_pitch = true; } else { - /* roll/pitch mixed without limiting, add yaw control */ + /* roll/pitch mixed without lower side limiting, add yaw control */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] += yaw * _rotors[i].yaw_scale; } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e70b83cce..035f63bef 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -39,7 +39,7 @@ #pragma once - __BEGIN_DECLS +__BEGIN_DECLS /** * Check the RC calibration diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b45c49788..73fe0f936 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,7 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index a61f078ba..43cee89fe 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -65,7 +65,7 @@ struct fence_vertex_s { * This is the position of a geofence * */ -struct fence_s { +struct fence_s { unsigned count; /**< number of actual vertices */ struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index 7366d7fc6..125ceaddc 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -66,6 +66,7 @@ int DemoOffboardPositionSetpoints::main() pose.pose.position.z = 1; _local_position_sp_pub.publish(pose); } + return 0; } diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c deleted file mode 100644 index 3ff5a066c..000000000 --- a/src/systemcmds/boardinfo/boardinfo.c +++ /dev/null @@ -1,409 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file boardinfo.c - * autopilot and carrier board information app - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <string.h> -#include <stdlib.h> - -#include <nuttx/i2c.h> - -#include <systemlib/systemlib.h> -#include <systemlib/err.h> -#include <systemlib/bson/tinybson.h> - -__EXPORT int boardinfo_main(int argc, char *argv[]); - -struct eeprom_info_s -{ - unsigned bus; - unsigned address; - unsigned page_size; - unsigned page_count; - unsigned page_write_delay; -}; - -/* XXX currently code below only supports 8-bit addressing */ -const struct eeprom_info_s eeprom_info[] = { - {3, 0x57, 8, 16, 3300}, -}; - -struct board_parameter_s { - const char *name; - bson_type_t type; -}; - -const struct board_parameter_s board_parameters[] = { - {"name", BSON_STRING}, /* ascii board name */ - {"vers", BSON_INT32}, /* board version (major << 8) | minor */ - {"date", BSON_INT32}, /* manufacture date */ - {"build", BSON_INT32} /* build code (fab << 8) | tester */ -}; - -const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); - -static int -eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - uint8_t pagebuf[eeprom->page_size + 1]; - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain writes to the page size */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - pagebuf[0] = address & 0xff; - memcpy(pagebuf + 1, buf + address, count); - - struct i2c_msg_s msgv[1] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = pagebuf, - .length = count + 1 - } - }; - - result = I2C_TRANSFER(dev, msgv, 1); - if (result != OK) { - warnx("EEPROM write failed: %d", result); - goto out; - } - usleep(eeprom->page_write_delay); - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static int -eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain transfers to the page size (bus anti-hog) */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - uint8_t addr = address; - struct i2c_msg_s msgv[2] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = &addr, - .length = 1 - }, - { - .addr = eeprom->address, - .flags = I2C_M_READ, - .buffer = buf + address, - .length = count - } - }; - - result = I2C_TRANSFER(dev, msgv, 2); - if (result != OK) { - warnx("EEPROM read failed: %d", result); - goto out; - } - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static void * -idrom_read(const struct eeprom_info_s *eeprom) -{ - uint32_t size = 0xffffffff; - int result; - void *buf = NULL; - - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - goto fail; - } - if (size > (eeprom->page_size * eeprom->page_count)) { - warnx("ID ROM not programmed"); - goto fail; - } - - buf = malloc(size); - if (buf == NULL) { - warnx("could not allocate %d bytes for ID ROM", size); - goto fail; - } - result = eeprom_read(eeprom, buf, size); - if (result != 0) { - warnx("failed reading ID ROM"); - goto fail; - } - return buf; - -fail: - if (buf != NULL) - free(buf); - return NULL; -} - -static void -boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) -{ - struct bson_encoder_s encoder; - int result = 1; - char *state, *token; - unsigned i; - - /* create the encoder and make a writable copy of the spec */ - bson_encoder_init_buf(&encoder, NULL, 0); - - for (i = 0, token = strtok_r(spec, ",", &state); - token && (i < num_parameters); - i++, token = strtok_r(NULL, ",", &state)) { - - switch (board_parameters[i].type) { - case BSON_STRING: - result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); - break; - case BSON_INT32: - result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); - break; - default: - result = 1; - } - if (result) { - warnx("bson append failed for %s<%s>", board_parameters[i].name, token); - goto out; - } - } - bson_encoder_fini(&encoder); - if (i != num_parameters) { - warnx("incorrect parameter list, expected: \"<name>,<version><date>,<buildcode>\""); - result = 1; - goto out; - } - if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { - warnx("data too large for EEPROM"); - result = 1; - goto out; - } - if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { - warnx("buffer length mismatch"); - result = 1; - goto out; - } - warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - - result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - if (result < 0) { - warnc(-result, "error writing EEPROM"); - result = 1; - } else { - result = 0; - } - -out: - free(bson_encoder_buf_data(&encoder)); - - exit(result); -} - -static int -boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) -{ - switch (node->type) { - case BSON_INT32: - printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); - break; - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - printf("%s: %s\n", node->name, buf); - break; - } - case BSON_EOO: - break; - default: - warnx("unexpected node type %d", node->type); - break; - } - return 1; -} - -static void -boardinfo_show(const struct eeprom_info_s *eeprom) -{ - struct bson_decoder_s decoder; - void *buf; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { - while (bson_decoder_next(&decoder) > 0) - ; - } else { - warnx("failed to init decoder"); - } - free(buf); - exit(0); -} - -struct { - const char *property; - const char *value; -} test_args; - -static int -boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - /* reject nodes with non-matching names */ - if (strcmp(node->name, test_args.property)) - return 1; - - /* compare node values to check for a match */ - switch (node->type) { - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - - /* check for a match */ - if (!strcmp(test_args.value, buf)) { - return 2; - } - break; - } - - case BSON_INT32: { - int32_t val = strtol(test_args.value, NULL, 0); - - /* check for a match */ - if (node->i == val) { - return 2; - } - break; - } - - default: - break; - } - - return 1; -} - -static void -boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) -{ - struct bson_decoder_s decoder; - void *buf; - int result = -1; - - if ((property == NULL) || (strlen(property) == 0) || - (value == NULL) || (strlen(value) == 0)) - errx(1, "missing property name or value"); - - test_args.property = property; - test_args.value = value; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { - do { - result = bson_decoder_next(&decoder); - } while (result == 1); - } else { - warnx("failed to init decoder"); - } - free(buf); - - /* if we matched, we exit with zero success */ - exit((result == 2) ? 0 : 1); -} - -int -boardinfo_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "set")) - boardinfo_set(&eeprom_info[0], argv[2]); - - if (!strcmp(argv[1], "show")) - boardinfo_show(&eeprom_info[0]); - - if (!strcmp(argv[1], "test")) - boardinfo_test(&eeprom_info[0], argv[2], argv[3]); - - errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); -} diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk deleted file mode 100644 index 6851d229b..000000000 --- a/src/systemcmds/boardinfo/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Information about FMU and IO boards connected -# - -MODULE_COMMAND = boardinfo -SRCS = boardinfo.c - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index aee26680c..d74010553 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -168,8 +168,13 @@ esc_calib_main(int argc, char *argv[]) } } - if (set_mask == 0) + if (set_mask == 0) { usage("no channels chosen"); + } + + if (pwm_low > pwm_high) { + usage("low pwm is higher than high pwm"); + } /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1d728e857..4b7b73ac6 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,7 +32,8 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c + test_mount.c \ + test_eigen.cpp EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp new file mode 100644 index 000000000..de5630cc3 --- /dev/null +++ b/src/systemcmds/tests/test_eigen.cpp @@ -0,0 +1,422 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_eigen.cpp + * + * Eigen test (based of mathlib test) + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include <px4_eigen.h> +#include <float.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <time.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include "tests.h" + +namespace Eigen +{ + typedef Matrix<float, 10, 1> Vector10f; +} + +static constexpr size_t OPERATOR_ITERATIONS = 60000; + +#define TEST_OP(_title, _op) \ +{ \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ +} + +#define VERIFY_OP(_title, _op, __OP_TEST__) \ +{ \ + _op; \ + if(!(__OP_TEST__)) { \ + printf(_title " Failed! ("#__OP_TEST__")"); \ + } \ +} + +#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ + VERIFY_OP(_title, _op, __OP_TEST__) \ + TEST_OP(_title, _op) + +/** +* @brief +* Prints an Eigen::Matrix to stdout +**/ +template<typename T> +void printEigen(const Eigen::MatrixBase<T>& b) +{ + for(int i = 0; i < b.rows(); ++i) { + printf("("); + for(int j = 0; j < b.cols(); ++i) { + if(j > 0) { + printf(","); + } + + printf("%.3f", static_cast<double>(b(i, j))); + } + printf(")%s\n", i+1 < b.rows() ? "," : ""); + } +} + +/** +* @brief +* Construct new Eigen::Quaternion from euler angles +**/ +template<typename T> +Eigen::Quaternion<T> quatFromEuler(const T roll, const T pitch, const T yaw) +{ + Eigen::AngleAxis<T> rollAngle(roll, Eigen::Matrix<T, 3, 1>::UnitZ()); + Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Matrix<T, 3, 1>::UnitY()); + Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Matrix<T, 3, 1>::UnitX()); + return rollAngle * yawAngle * pitchAngle; +} + + +int test_eigen(int argc, char *argv[]) +{ + int rc = 0; + warnx("testing eigen"); + + { + Eigen::Vector2f v; + Eigen::Vector2f v1(1.0f, 2.0f); + Eigen::Vector2f v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); + TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1)); + TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); + TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); + TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); + //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? + } + + { + Eigen::Vector3f v; + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + Eigen::Vector3f v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3); + TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1)); + TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data)); + TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector3f = Vector3f", v = v1); + TEST_OP("Vector3f + Vector3f", v + v1); + TEST_OP("Vector3f - Vector3f", v - v1); + TEST_OP("Vector3f += Vector3f", v += v1); + TEST_OP("Vector3f -= Vector3f", v -= v1); + TEST_OP("Vector3f * float", v1 * 2.0f); + TEST_OP("Vector3f / float", v1 / 2.0f); + TEST_OP("Vector3f *= float", v1 *= 2.0f); + TEST_OP("Vector3f /= float", v1 /= 2.0f); + TEST_OP("Vector3f dot Vector3f", v.dot(v1)); + TEST_OP("Vector3f cross Vector3f", v1.cross(v2)); + TEST_OP("Vector3f length", v1.norm()); + TEST_OP("Vector3f length squared", v1.squaredNorm()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]); +#pragma GCC diagnostic pop + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f); + } + + { + Eigen::Vector4f v; + Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); + Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); + } + + { + Eigen::Vector10f v1; + v1.Zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + } + + { + Eigen::Matrix3f m1; + m1.setIdentity(); + Eigen::Matrix3f m2; + m2.setIdentity(); + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix3f * Vector3f", m1 * v1); + TEST_OP("Matrix3f + Matrix3f", m1 + m2); + TEST_OP("Matrix3f * Matrix3f", m1 * m2); + } + + { + Eigen::Matrix<float, 10, 10> m1; + m1.setIdentity(); + Eigen::Matrix<float, 10, 10> m2; + m2.setIdentity(); + Eigen::Vector10f v1; + v1.Zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + { + warnx("Nonsymmetric matrix operations test"); + // test nonsymmetric +, -, +=, -= + + const Eigen::Matrix<float, 2, 3> m1_orig = + (Eigen::Matrix<float, 2, 3>() << 1, 3, 3, + 4, 6, 6).finished(); + + Eigen::Matrix<float, 2, 3> m1(m1_orig); + + Eigen::Matrix<float, 2, 3> m2; + m2 << 2, 4, 6, + 8, 10, 12; + + Eigen::Matrix<float, 2, 3> m3; + m3 << 3, 6, 9, + 12, 15, 18; + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printEigen(m1 + m2); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printEigen(m3 - m2); + printf("!=\n"); + printEigen(m1); + rc = 1; + } + + m1 += m2; + + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + m1 -= m2; + + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m1_orig); + rc = 1; + } + + } + + { + // test conversion rotation matrix to quaternion and back + Eigen::Matrix3f R_orig; + Eigen::Matrix3f R; + Eigen::Quaternionf q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.eulerAngles(roll, pitch, yaw); + q = R_orig; //from_dcm + R = q.toRotationMatrix(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.setIdentity(); + q = R_orig; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + } + + { + // test quaternion method "rotate" (rotate vector by quaternion) + Eigen::Vector3f vector = {1.0f,1.0f,1.0f}; + Eigen::Vector3f vector_q; + Eigen::Vector3f vector_r; + Eigen::Quaternionf q; + Eigen::Matrix3f R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion vector rotation method test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R.eulerAngles(roll, pitch, yaw); + q = quatFromEuler(roll,pitch,yaw); + vector_r = R*vector; + vector_q = q._transformVector(vector); + for (int i = 0; i < 3; i++) { + if(fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + q = quatFromEuler(M_PI_2_F,0.0f,0.0f); + vector_q = q._transformVector(vector); + Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(0.3f,0.2f,0.1f); + vector_q = q._transformVector(vector); + vector_true = {1.1566,0.7792,1.0273}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(-1.5f,-0.2f,0.5f); + vector_q = q._transformVector(vector); + vector_true = {0.5095,1.4956,-0.7096}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + vector_q = q._transformVector(vector); + vector_true = {-1.3660,0.3660,1.0000}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + return rc; +}
\ No newline at end of file diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 98a105cb3..554125302 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -162,7 +162,7 @@ int test_jig_voltages(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return ret; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index a753f45c2..6866531d7 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -105,16 +105,15 @@ int test_ppm_loopback(int argc, char *argv[]) int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - - // if (result) { - // (void)close(servo_fd); - // return ERROR; - // } else { - // warnx("channel %d set to %d", i, pwm_values[i]); - // } - // } + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } else { + warnx("channel %d set to %d", i, pwm_values[i]); + } + } warnx("servo count: %d", servo_count); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index c9f9ef439..3a8144077 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -126,7 +126,7 @@ int test_rc(int argc, char *argv[]) warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; - } + } } else { /* key pressed, bye bye */ diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 7e1e8d307..70a9c719e 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -95,7 +95,7 @@ int test_uart_send(int argc, char *argv[]) /* input handling */ char *uart_name = "/dev/ttyS3"; - if (argc > 1) uart_name = argv[1]; + if (argc > 1) { uart_name = argv[1]; } /* assuming NuttShell is on UART1 (/dev/ttyS0) */ int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 69fb0e57b..9d13d50d7 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -113,6 +113,7 @@ extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); +extern int test_eigen(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0768c1d5a..7ffd47892 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif + {"eigen", test_eigen, OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 98bbbc4be..530fee340 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]); int usb_connected_main(int argc, char *argv[]) { - return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; } diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index a27d5f100..8c89243f6 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -1,6 +1,6 @@ #include <math.h> #include <stdio.h> -#include <stdlib.h> +#include <stdlib.h> #include <string.h> #include <unistd.h> @@ -12,6 +12,7 @@ #include "gtest/gtest.h" -TEST(AutoDeclinationTest, AutoDeclination) { +TEST(AutoDeclinationTest, AutoDeclination) +{ ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree"; } diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp index 12d2213e2..7a2909dc3 100644 --- a/unittests/conversion_test.cpp +++ b/unittests/conversion_test.cpp @@ -4,6 +4,7 @@ #include "gtest/gtest.h" -TEST(ConversionTest, quad_w_main) { - ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; +TEST(ConversionTest, quad_w_main) +{ + ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; } diff --git a/unittests/hrt.cpp b/unittests/hrt.cpp index 01b5958b7..d7b4670db 100644 --- a/unittests/hrt.cpp +++ b/unittests/hrt.cpp @@ -3,14 +3,16 @@ #include <drivers/drv_hrt.h> #include <stdio.h> -hrt_abstime hrt_absolute_time() { - struct timeval te; - gettimeofday(&te, NULL); // get current time - hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us - return us; +hrt_abstime hrt_absolute_time() +{ + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; } -hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ // not thread safe - return hrt_absolute_time() - *then; + return hrt_absolute_time() - *then; } diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 1271dab5e..5be4cb032 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -5,7 +5,8 @@ #include "gtest/gtest.h" -TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; +TEST(MixerTest, Mixer) +{ + char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 5ea6bcc60..44ae4df06 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -13,7 +13,8 @@ struct param_info_s *param_info_limit; /* * Adds test parameters */ -void _add_parameters() { +void _add_parameters() +{ struct param_info_s test_1 = { "TEST_1", PARAM_TYPE_INT32 @@ -44,17 +45,19 @@ void _add_parameters() { param_array[3] = rc2_x; param_info_base = (struct param_info_s *) ¶m_array[0]; param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 + // therefore number of params + 1 } -void _assert_parameter_int_value(param_t param, int32_t expected) { +void _assert_parameter_int_value(param_t param, int32_t expected) +{ int32_t value; int result = param_get(param, &value); ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); } -void _set_all_int_parameters_to(int32_t value) { +void _set_all_int_parameters_to(int32_t value) +{ param_set((param_t)0, &value); param_set((param_t)1, &value); param_set((param_t)2, &value); @@ -66,9 +69,10 @@ void _set_all_int_parameters_to(int32_t value) { _assert_parameter_int_value((param_t)3, value); } -TEST(ParamTest, SimpleFind) { +TEST(ParamTest, SimpleFind) +{ _add_parameters(); - + param_t param = param_find("TEST_2"); ASSERT_NE(PARAM_INVALID, param) << "param_find did not find parameter"; @@ -78,7 +82,8 @@ TEST(ParamTest, SimpleFind) { ASSERT_EQ(4, value) << "value of returned parameter does not match"; } -TEST(ParamTest, ResetAll) { +TEST(ParamTest, ResetAll) +{ _add_parameters(); _set_all_int_parameters_to(50); @@ -90,11 +95,12 @@ TEST(ParamTest, ResetAll) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesOne) { +TEST(ParamTest, ResetAllExcludesOne) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X"}; + const char *excludes[] = {"RC_X"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -103,11 +109,12 @@ TEST(ParamTest, ResetAllExcludesOne) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesTwo) { +TEST(ParamTest, ResetAllExcludesTwo) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 2); _assert_parameter_int_value((param_t)0, 50); @@ -116,11 +123,12 @@ TEST(ParamTest, ResetAllExcludesTwo) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesBoundaryCheck) { +TEST(ParamTest, ResetAllExcludesBoundaryCheck) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -129,11 +137,12 @@ TEST(ParamTest, ResetAllExcludesBoundaryCheck) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesWildcard) { +TEST(ParamTest, ResetAllExcludesWildcard) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC*"}; + const char *excludes[] = {"RC*"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); diff --git a/unittests/queue.h b/unittests/queue.h index 0fdb170db..e056bc263 100644 --- a/unittests/queue.h +++ b/unittests/queue.h @@ -67,30 +67,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -110,11 +106,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 15a21c86b..41f49627d 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -10,11 +10,12 @@ #include "gtest/gtest.h" -TEST(SBUS2Test, SBUS2) { +TEST(SBUS2Test, SBUS2) +{ const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; - fp = fopen(filepath,"rt"); + fp = fopen(filepath, "rt"); ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); @@ -51,8 +52,9 @@ TEST(SBUS2Test, SBUS2) { //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - if (partial_frame_count == sizeof(frame)) + if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; + } last_time = f; @@ -60,7 +62,7 @@ TEST(SBUS2Test, SBUS2) { hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } ASSERT_EQ(ret, EOF); diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index b161f1ffd..56e673a06 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -9,7 +9,8 @@ #include "gtest/gtest.h" -TEST(SF0XTest, SF0X) { +TEST(SF0XTest, SF0X) +{ const char _LINE_MAX = 20; char _linebuf[_LINE_MAX]; _linebuf[0] = '\0'; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 89c7ffb1c..42fa07ae7 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(ST24Test, ST24) { - const char* filepath = "testdata/st24_data.txt"; +TEST(ST24Test, ST24) +{ + const char *filepath = "testdata/st24_data.txt"; warnx("loading data from: %s", filepath); diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp index fc039a408..e4269afc3 100644 --- a/unittests/uorb_stub.cpp +++ b/unittests/uorb_stub.cpp @@ -10,11 +10,13 @@ * TODO: use googlemock ******************************************/ -orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ return (orb_advert_t)0; } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ return 0; } |