diff --git a/.cproject b/.cproject index f90fe5d114..a0577f3329 100644 --- a/.cproject +++ b/.cproject @@ -77,7 +77,7 @@ - + diff --git a/Makefile b/Makefile index 4632699433..a0dc23f705 100644 --- a/Makefile +++ b/Makefile @@ -118,6 +118,13 @@ ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/qurt/firmware_qurt.mk endif +qurt_fixup: + makefiles/qurt/setup.sh $(PX4_BASE) + +restore: + cd src/lib/eigen && git checkout . + git submodule update + # # Versioning # @@ -155,7 +162,7 @@ size: .PHONY: checksubmodules checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh $(PX4_TARGET_OS)) .PHONY: updatesubmodules updatesubmodules: diff --git a/NuttX b/NuttX index 8004677d8c..8891d035df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 8004677d8c9222685f331409469e78a66375d55f +Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 08ba86d789..37e6893bd1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,16 +3,50 @@ # UAVCAN initialization script. # +# +# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable. +# TODO there should be a smarter way. +# +set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 +then + set UAVCAN_ENABLE 1 +fi +if param compare UAVCAN_ENABLE 2 +then + set UAVCAN_ENABLE 2 +fi + +echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" + +# +# Starting stuff according to UAVCAN_ENABLE value +# +if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs - sleep 1 echo "[i] UAVCAN started" else echo "[i] ERROR: Could not start UAVCAN" tone_alarm $TUNE_ERR fi fi + +if [ $UAVCAN_ENABLE -ge 2 ] +then + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi +fi + +if [ $UAVCAN_ENABLE -ge 1 ] +then + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs + sleep 8 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index c1c1b5e57c..4a157a683f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,26 +3,7 @@ # USB MAVLink start # -mavlink start -r 800000 -d /dev/ttyACM0 -x -# Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 -mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 -mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 -mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 -mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 -mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 -mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS -r 5 -mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 -mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_1 -r 20 -mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 -mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 -mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 -mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 -mavlink stream -d /dev/ttyACM0 -s CAMERA_TRIGGER -r 500 -mavlink stream -d /dev/ttyACM0 -s VTOL_STATE -r 2 +mavlink start -r 800000 -d /dev/ttyACM0 -m config -x # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e532445531..85cbdeb59e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -89,6 +89,70 @@ then fi fi + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_THROTTLE 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC3_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_THROTTLE 3 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_ROLL 0 + then + # So this is a default setup, + # now lets find out if channel 1 + # is calibrated + if param compare RC1_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_ROLL 1 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_PITCH 0 + then + # So this is a default setup, + # now lets find out if channel 2 + # is calibrated + if param compare RC2_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_PITCH 2 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_YAW 0 + then + # So this is a default setup, + # now lets find out if channel 4 + # is calibrated + if param compare RC4_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_YAW 4 + fi + fi + # # Start system state indicator # @@ -287,6 +351,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set GPS no if ver hwcmp PX4FMU_V1 then set FMU_MODE serial @@ -300,6 +365,11 @@ then then fi + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors System (start before Commander so Preflight checks are properly run) # @@ -315,9 +385,12 @@ then gps start fi fi + unset GPS unset GPS_FAKE # Needs to be this early for in-air-restarts + # Wait 10 ms for sensors (workaround for airspeed, to be removed) + usleep 10000 commander start # @@ -505,10 +578,6 @@ then fi fi - # UAVCAN - # - sh /etc/init.d/rc.uavcan - # # Logging # diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b251642ce..5fa0831438 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -70,30 +68,35 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d src/lib/eigen ] then - STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") - if [ -z "$STATUSRETVAL" ] + echo "ARG = $1" + if [ $1 = "qurt" ] then - echo "Checked Eigen submodule, correct version found" + # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 + STATUSRETVAL=$(true) 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 + 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 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/gencpp ] @@ -114,8 +117,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/genmsg ] @@ -136,8 +138,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi exit 0 diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 918040bbf5..c69699cad7 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +global default_var +default_var = {} class ParameterGroup(object): """ @@ -98,6 +100,7 @@ class SourceParser(object): re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') @@ -114,13 +117,6 @@ class SourceParser(object): def __init__(self): self.param_groups = {} - def GetSupportedExtensions(self): - """ - Returns list of supported file extensions that can be parsed by this - parser. - """ - return [".cpp", ".c"] - def Parse(self, contents): """ Incrementally parse program contents and append all found parameters @@ -193,6 +189,10 @@ class SourceParser(object): name = None defval = "" # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + if m: + name_m, defval_m = m.group(1,2) + default_var[name_m] = defval_m m = self.re_parameter_definition.match(line) if m: tp, name, defval = m.group(1, 2, 3) @@ -200,6 +200,8 @@ class SourceParser(object): m = self.re_px4_parameter_definition.match(line) if m: tp, name = m.group(1, 2) + if default_var.has_key(name+'_DEFAULT'): + defval = default_var[name+'_DEFAULT'] if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 if defval != "" and self.re_is_a_number.match(defval): diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index a49039c719..4db5698992 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -13,10 +13,16 @@ class SourceScanner(object): Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = tuple(parser.GetSupportedExtensions()) + extensions1 = tuple([".h"]) + extensions2 = tuple([".cpp", ".c"]) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - if filename.endswith(extensions): + if filename.endswith(extensions1): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + for filename in filenames: + if filename.endswith(extensions2): path = os.path.join(dirname, filename) if not self.ScanFile(path, parser): return False diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 6e2cbc19be..3c65aa4eec 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -2,4 +2,4 @@ mkdir -p Build/posix_sitl.build/rootfs/fs/microsd mkdir -p Build/posix_sitl.build/rootfs/eeprom -cd Build/posix_sitl.build && ./mainapp ../$1 +cd Build/posix_sitl.build && ./mainapp ../../$1 diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 4b436072ba..c59b66b21f 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -181,6 +181,12 @@ EXTRA_CLEANS = # INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) +################################################################################ +# External library includes +################################################################################ + +INCLUDE_DIRS += $(PX4_BASE)src/lib/eigen/ + ################################################################################ # OS specific libraries and paths ################################################################################ diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index f22589076f..8d16e538a0 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -143,19 +143,19 @@ MODULES += examples/rover_steering_control # #MODULES += examples/math_demo # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky +# https://px4.io/dev/px4_simple_app #MODULES += examples/px4_simple_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon +# https://px4.io/dev/daemon #MODULES += examples/px4_daemon_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values +# https://px4.io/dev/debug_values #MODULES += examples/px4_mavlink_debug # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +# https://px4.io/dev/example_fixedwing_control #MODULES += examples/fixedwing_control # Hardware test diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 6b6708a5cc..57ede0b78b 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -49,6 +49,10 @@ MODULES += modules/mc_att_control MODULES += modules/mc_pos_control_multiplatform MODULES += modules/mc_att_control_multiplatform MODULES += modules/land_detector +MODULES += modules/fw_att_control +MODULES += modules/fw_pos_control_l1 + + # # Library modules @@ -67,9 +71,13 @@ MODULES += modules/controllib # MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += lib/launchdetection + # # POSIX port diff --git a/makefiles/qurt/config_qurt_adsp.mk b/makefiles/qurt/config_qurt_adsp.mk index 75b1bc429f..6a1ce1d5f2 100644 --- a/makefiles/qurt/config_qurt_adsp.mk +++ b/makefiles/qurt/config_qurt_adsp.mk @@ -1,3 +1,9 @@ +#Added configuration specific flags here. + +# For Actual flight we need to link against the driver dynamic libraries +LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 +LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc + # # Makefile for the EAGLE QuRT *default* configuration # diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index ee0288d764..0536d7a4ad 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -57,6 +57,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += modules/controllib # # QuRT port diff --git a/makefiles/qurt/setup.sh b/makefiles/qurt/setup.sh new file mode 100755 index 0000000000..8ffb13cc4b --- /dev/null +++ b/makefiles/qurt/setup.sh @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 Mark Charlebois. 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. +# +# ########################################################################### + +# +# This file is required to modify the PX4 project so it can be build with +# the Hexagon toolchain. +# +# 1. Downgrade Eigen to 3.2 because the C++11 features of latest version +# are not supported +# 2. Patch Eigen because the Hexagon toolchain complex.h defines "I" +# + +cd $1 +cd src/lib/eigen/ +git checkout . +git checkout e7850ed81f9c469e02df496ef09ae32ec0379b71 +patch -p1 < ../../../makefiles/qurt/qurt_eigen.patch + diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 556c2e21dc..038c299904 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -246,10 +246,6 @@ LDFLAGS += -g -mv5 -mG0lib -G0 -fpic -shared \ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) -# driver dynamic libraries -LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 -LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc - # Compiler support library # LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index e35b4c8e51..c390a63abf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit e35b4c8e5171b2ee4a37ae0d6f240c887c455227 +Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118 diff --git a/msg/input_rc.msg b/msg/input_rc.msg new file mode 100644 index 0000000000..c07d229503 --- /dev/null +++ b/msg/input_rc.msg @@ -0,0 +1,21 @@ +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_publication # publication time +uint64 timestamp_last_signal # last valid reception time +uint32 channel_count # number of channels actually being seen +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels diff --git a/msg/mission.msg b/msg/mission.msg new file mode 100644 index 0000000000..ac135a4e09 --- /dev/null +++ b/msg/mission.msg @@ -0,0 +1,3 @@ +int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint32 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest diff --git a/msg/output_pwm.msg b/msg/output_pwm.msg new file mode 100644 index 0000000000..1418a43e6e --- /dev/null +++ b/msg/output_pwm.msg @@ -0,0 +1,3 @@ +uint8 PWM_OUTPUT_MAX_CHANNELS = 16 +uint16[16] values +uint32 channel_count diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg new file mode 100644 index 0000000000..b2dd1843ab --- /dev/null +++ b/msg/rc_parameter_map.msg @@ -0,0 +1,11 @@ +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +uint64 timestamp # time at which the map was updated +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # inital value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg new file mode 100644 index 0000000000..fdae381aba --- /dev/null +++ b/msg/sensor_accel.msg @@ -0,0 +1,13 @@ +uint64 timestamp +uint64 error_count +float32 x # acceleration in the NED X board axis in m/s^2 +float32 y # acceleration in the NED Y board axis in m/s^2 +float32 z # acceleration in the NED Z board axis in m/s^2 +float32 temperature # temperature in degrees celsius +float32 range_m_s2 # range in m/s^2 (+- this value) +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg new file mode 100644 index 0000000000..315d5a56ec --- /dev/null +++ b/msg/sensor_baro.msg @@ -0,0 +1,5 @@ +float32 pressure +float32 altitude +float32 temperature +uint64 timestamp +uint64 error_count diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg new file mode 100644 index 0000000000..4a252269e8 --- /dev/null +++ b/msg/sensor_gyro.msg @@ -0,0 +1,13 @@ +uint64 timestamp +uint64 error_count +float32 x # angular velocity in the NED X board axis in rad/s +float32 y # angular velocity in the NED Y board axis in rad/s +float32 z # angular velocity in the NED Z board axis in rad/s +float32 temperature # temperature in degrees celcius +float32 range_rad_s +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg new file mode 100644 index 0000000000..3ac97227ea --- /dev/null +++ b/msg/sensor_mag.msg @@ -0,0 +1,12 @@ +uint64 timestamp +uint64 error_count +float32 x +float32 y +float32 z +float32 range_ga +float32 scaling +float32 temperature + +int16 x_raw +int16 y_raw +int16 z_raw diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index ccac921e2b..4dcd16b2ca 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -18,9 +18,12 @@ float32 airspeed_filtered float32 airspeedDerivativeSp float32 airspeedDerivative -float32 totalEnergyRateSp -float32 totalEnergyRate -float32 energyDistributionRateSp -float32 energyDistributionRate +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_integ +float32 pitch_integ uint8 mode diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e828e42be0..59abc22fb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -564,7 +564,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=1860 +CONFIG_USART2_TXBUFSIZE=1100 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -575,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=400 -CONFIG_USART3_TXBUFSIZE=400 +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -587,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=400 -CONFIG_UART4_TXBUFSIZE=400 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -599,8 +599,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=400 -CONFIG_USART6_TXBUFSIZE=400 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -611,8 +611,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=400 -CONFIG_UART7_TXBUFSIZE=400 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -623,8 +623,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=400 -CONFIG_UART8_TXBUFSIZE=400 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=5000 +CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index a9b8477948..56892e3f93 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,198 +1,705 @@ -############################################################################ -# configs/px4io/nsh/defconfig # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: +CONFIG_NUTTX_NEWCONFIG=y + # -# 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 NuttX nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. +# Build Setup # -# 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. +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + # -############################################################################ +# Build Configuration # -# architecture selection +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + # -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization +# Binary Output Formats # -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io-v2" -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +# CONFIG_ARMV7M_USEBASEPRI is not set +CONFIG_ARCH_HAVE_CMNVECTOR=y CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_ARMV7M_STACKCHECK=n -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_ARCH_HAVE_FPU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set # -# Individual subsystems can be enabled: +# ARMV7M Configuration Options # -# AHB: +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_VALUELINE=y +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +# CONFIG_STM32_STM32F40XX is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CEC is not set +# CONFIG_STM32_CRC is not set CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_I2C1 is not set +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_TIM15 is not set +# CONFIG_STM32_TIM16 is not set +# CONFIG_STM32_TIM17 is not set +CONFIG_STM32_USART1=y CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=n -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y # -# STM32F100 specific serial device driver settings +# Alternate Pin Mapping # -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits +# CONFIG_STM32_USART1_REMAP is not set +# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART3_NO_REMAP=y +# CONFIG_STM32_USART3_FULL_REMAP is not set +# CONFIG_STM32_USART3_PARTIAL_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_STM32_USART=y + # -CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y - -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +# CONFIG_USART2_RS485 is not set +# CONFIG_USART2_RXDMA is not set +# CONFIG_USART3_RS485 is not set CONFIG_USART3_RXDMA=y +# CONFIG_SERIAL_DISABLE_REORDERING is not set +# CONFIG_STM32_USART_SINGLEWIRE is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=2000 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=0x2000 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=500 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y + +# +# Common Board Options +# + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_TASK_NAME_SIZE=8 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +# CONFIG_SCHED_WAITPID is not set +# CONFIG_SCHED_STARTHOOK is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="user_start" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_ENVIRON=y + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=290 +CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 + +# +# Device Drivers +# +CONFIG_DISABLE_POLL=y +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +# CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_SPI is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +CONFIG_DISABLE_MOUNTPOINT=y +# CONFIG_FS_RAMMAP is not set + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +CONFIG_MM_SMALL=y +CONFIG_MM_REGIONS=1 +# CONFIG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set + +# +# USB Monitor +# # # PX4IO specific driver settings @@ -216,332 +723,3 @@ CONFIG_USART3_RXDMA=y CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=8 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# - -# Disable NSH completely -CONFIG_NSH_CONSOLE=n - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1200 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index a4a340cc38..65b69e4d0b 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -29,7 +29,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start fixedwing diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index f9518bfa02..6a9025bc97 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -34,7 +34,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start multicopter diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 9dc28a8505..558fa6464b 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -48,7 +48,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start multicopter diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index f2352648dc..0b6227a1be 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * 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 @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file ets_airspeed.cpp - * @author Simon Wilks + * @file airspeed.cpp + * @author Simon Wilks + * @author Lorenz Meier * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ @@ -76,7 +77,7 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -105,12 +106,14 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -124,13 +127,15 @@ Airspeed::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -146,8 +151,9 @@ Airspeed::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { warnx("uORB started?"); + } } ret = OK; @@ -166,7 +172,7 @@ Airspeed::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -179,20 +185,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -202,13 +208,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -217,15 +224,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -233,21 +242,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -261,16 +274,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +300,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +383,7 @@ Airspeed::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,6 +417,7 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0ed4d39ada..507cebbf83 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) /* yes? post the notification */ if (fds->revents != 0) sem_post(fds->sem); + } else { + PX4_WARN("Store Poll Waiter error."); } } else { @@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device - +} // namespace device \ No newline at end of file diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 3fd28c82ca..c39857bbf0 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -324,6 +324,13 @@ public: static const char *devList(unsigned int *next); static const char *topicList(unsigned int *next); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + protected: int register_driver(const char *name, void *data); @@ -406,13 +413,6 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - bool _pub_blocked; /**< true if publishing should be blocked */ private: diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 845050edf5..3af204edcd 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -51,29 +51,8 @@ #define ACCEL1_DEVICE_PATH "/dev/accel1" #define ACCEL2_DEVICE_PATH "/dev/accel2" -/** - * accel report structure. Reads from the device must be in multiples of this - * structure. - */ -struct accel_report { - uint64_t timestamp; - uint64_t integral_dt; /**< integration time */ - uint64_t error_count; - float x; /**< acceleration in the NED X board axis in m/s^2 */ - float y; /**< acceleration in the NED Y board axis in m/s^2 */ - float z; /**< acceleration in the NED Z board axis in m/s^2 */ - float x_integral; /**< velocity in the NED X board axis in m/s over the integration time frame */ - float y_integral; /**< velocity in the NED Y board axis in m/s over the integration time frame */ - float z_integral; /**< velocity in the NED Z board axis in m/s over the integration time frame */ - float temperature; /**< temperature in degrees celsius */ - float range_m_s2; /**< range in m/s^2 (+- this value) */ - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define accel_report sensor_accel_s /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { @@ -85,11 +64,6 @@ struct accel_scale { float z_scale; }; -/* - * ObjDev tag for raw accelerometer data. - */ -ORB_DECLARE(sensor_accel); - /* * ioctl() definitions * diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 791e3c5cc1..87bf1000ec 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -50,22 +50,8 @@ #define BARO_BASE_DEVICE_PATH "/dev/baro" #define BARO0_DEVICE_PATH "/dev/baro0" -/** - * baro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct baro_report { - float pressure; - float altitude; - float temperature; - uint64_t timestamp; - uint64_t error_count; -}; - -/* - * ObjDev tag for raw barometer data. - */ -ORB_DECLARE(sensor_baro); +#include +#define baro_report sensor_baro_s /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 60426caf90..2ed65802cd 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -51,29 +51,8 @@ #define GYRO1_DEVICE_PATH "/dev/gyro1" #define GYRO2_DEVICE_PATH "/dev/gyro2" -/** - * gyro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct gyro_report { - uint64_t timestamp; - uint64_t integral_dt; /**< integration time */ - uint64_t error_count; - float x; /**< angular velocity in the NED X board axis in rad/s */ - float y; /**< angular velocity in the NED Y board axis in rad/s */ - float z; /**< angular velocity in the NED Z board axis in rad/s */ - float x_integral; /**< delta angle in the NED X board axis in rad/s in the integration time frame */ - float y_integral; /**< delta angle in the NED Y board axis in rad/s in the integration time frame */ - float z_integral; /**< delta angle in the NED Z board axis in rad/s in the integration time frame */ - float temperature; /**< temperature in degrees celcius */ - float range_rad_s; - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define gyro_report sensor_gyro_s /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ struct gyro_scale { @@ -85,11 +64,6 @@ struct gyro_scale { float z_scale; }; -/* - * ObjDev tag for raw gyro data. - */ -ORB_DECLARE(sensor_gyro); - /* * ioctl() definitions */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e2f8493e38..75d914de8e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -49,26 +49,8 @@ #define MAG1_DEVICE_PATH "/dev/mag1" #define MAG2_DEVICE_PATH "/dev/mag2" -/** - * mag report structure. Reads from the device must be in multiples of this - * structure. - * - * Output values are in gauss. - */ -struct mag_report { - uint64_t timestamp; - uint64_t error_count; - float x; - float y; - float z; - float range_ga; - float scaling; - float temperature; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; -}; +#include +#define mag_report sensor_mag_s /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ struct mag_scale { @@ -80,12 +62,6 @@ struct mag_scale { float z_scale; }; -/* - * ObjDev tag for raw magnetometer data. - */ -ORB_DECLARE(sensor_mag); - - /* * ioctl() definitions */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index eda113f5e4..fc619866be 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -61,10 +61,17 @@ __BEGIN_DECLS #define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output" #define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0" +#include +#define pwm_output_values output_pwm_s + +#ifndef PWM_OUTPUT_MAX_CHANNELS + #define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS +#endif + /** * Maximum number of PWM output channels supported by the device. */ -#define PWM_OUTPUT_MAX_CHANNELS 16 +//#define PWM_OUTPUT_MAX_CHANNELS 16 /** * Lowest minimum PWM in us @@ -107,19 +114,6 @@ __BEGIN_DECLS */ typedef uint16_t servo_position_t; -/** - * Servo output status structure. - * - * May be published to output_pwm, or written to a PWM output - * device. - */ -struct pwm_output_values { - /** desired pulse widths for each of the supported channels */ - servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - unsigned channel_count; -}; - - /** * RC config values for a channel * @@ -136,11 +130,6 @@ struct pwm_output_rc_config { bool rc_reverse; }; -/* - * ORB tag for PWM outputs. - */ -ORB_DECLARE(output_pwm); - /* * ioctl() definitions * diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 63cfe9816a..90bad2f25d 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,11 +46,6 @@ #define PX4FLOW0_DEVICE_PATH "/dev/px4flow0" -/* - * ObjDev tag for px4flow data. - */ -ORB_DECLARE(optical_flow); - /* * ioctl() definitions * diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 725835cc0a..7d27c7f33a 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,11 +48,6 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus -/* - * ObjDev tag for distance sensor data. - */ -ORB_DECLARE(distance_sensor); - /* * ioctl() definitions * diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index b82ced3845..b08f10d465 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -57,11 +57,6 @@ */ #define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" -/** - * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. - */ -#define RC_INPUT_MAX_CHANNELS 18 - /** * Maximum RSSI value */ @@ -82,10 +77,9 @@ */ #define RC_INPUT_MAX_DEADZONE_US 500 -/** - * @addtogroup topics - * @{ - */ +#include +#define pwm_output_values output_pwm_s +#define rc_input_values input_rc_s /** * Input signal type, value is a control position from zero to 100 @@ -93,85 +87,6 @@ */ typedef uint16_t rc_input_t; -enum RC_INPUT_SOURCE { - RC_INPUT_SOURCE_UNKNOWN = 0, - RC_INPUT_SOURCE_PX4FMU_PPM, - RC_INPUT_SOURCE_PX4IO_PPM, - RC_INPUT_SOURCE_PX4IO_SPEKTRUM, - RC_INPUT_SOURCE_PX4IO_SBUS, - RC_INPUT_SOURCE_PX4IO_ST24, - RC_INPUT_SOURCE_MAVLINK -}; - -/** - * R/C input status structure. - * - * Published to input_rc, may also be published to other names depending - * on the board involved. - */ -struct rc_input_values { - /** publication time */ - uint64_t timestamp_publication; - - /** last valid reception time */ - uint64_t timestamp_last_signal; - - /** number of channels actually being seen */ - uint32_t channel_count; - - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */ - int32_t rssi; - - /** - * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. - * Only the true state is reliable, as there are some (PPM) receivers on the market going - * into failsafe without telling us explicitly. - * */ - bool rc_failsafe; - - /** - * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. - * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. - * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. - * */ - bool rc_lost; - - /** - * Number of lost RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_lost_frame_count; - - /** - * Number of total RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_total_frame_count; - - /** - * Length of a single PPM frame. - * Zero for non-PPM systems - */ - uint16_t rc_ppm_frame_length; - - /** Input source */ - enum RC_INPUT_SOURCE input_source; - - /** measured pulse widths for each of the supported channels */ - rc_input_t values[RC_INPUT_MAX_CHANNELS]; -}; - -/** - * @} - */ - -/* - * ObjDev tag for R/C inputs. - */ -ORB_DECLARE(input_rc); - #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b4517d66e7..51c009717c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -94,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); protected: @@ -113,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -155,7 +155,8 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - if (diff_pres_pa_raw == 0) { + + if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the // caller could end up using this value as part of an @@ -163,7 +164,7 @@ ETSAirspeed::collect() perf_count(_comms_errors); DEVICE_LOG("zero value from sensor"); return -1; - } + } // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; @@ -175,7 +176,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); // XXX we may want to smooth out the readings to remove noise. report.differential_pressure_filtered_pa = diff_pres_pa_raw; @@ -210,6 +211,7 @@ ETSAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ @@ -239,6 +241,7 @@ ETSAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { DEVICE_DEBUG("measure error"); } @@ -287,26 +290,31 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new ETSAirspeed(i2c_bus); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -351,21 +359,24 @@ test() int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -376,22 +387,25 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -404,14 +418,17 @@ reset() { int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -422,8 +439,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { ets_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { ets_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ets_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { ets_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { ets_airspeed::info(); + } ets_airspeed_usage(); exit(0); diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 4aeb8c7497..19b0de6d5f 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -110,7 +110,7 @@ private: int read_device_block(struct irlock_s *block); /** internal variables **/ - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; @@ -158,7 +158,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a72bba77a2..ae77a07fd0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,7 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), + CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), _t_system_power(-1), system_power{} @@ -189,9 +189,11 @@ MEASAirspeed::collect() break; case 1: - /* fallthrough */ + + /* fallthrough */ case 2: - /* fallthrough */ + + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); @@ -219,11 +221,11 @@ MEASAirspeed::collect() are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ - float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa_raw, temperature); + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa_raw, temperature); // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; @@ -276,6 +278,7 @@ MEASAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); @@ -304,6 +307,7 @@ MEASAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { DEVICE_DEBUG("measure error"); } @@ -332,18 +336,23 @@ void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } + if (_t_system_power == -1) { // not available return; } + bool updated = false; orb_check(_t_system_power, &updated); + if (updated) { orb_copy(ORB_ID(system_power), _t_system_power, &system_power); } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { // not valid, skip correction return; @@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) apply a piecewise linear correction, flattening at 0.5V from 5V */ float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -0.5f) { voltage_diff = -0.5f; } + diff_press_pa -= voltage_diff * slope; /* @@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) */ const float temp_slope = 0.887f; voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } diff --git a/src/drivers/pwm_out_sim/module.mk b/src/drivers/pwm_out_sim/module.mk index 650f5676be..286b4d21f1 100644 --- a/src/drivers/pwm_out_sim/module.mk +++ b/src/drivers/pwm_out_sim/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = pwm_out_sim SRCS = pwm_out_sim.cpp + MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1200 diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index eaebef2e55..f42d166535 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -234,7 +234,7 @@ PWMSim::init() _task = px4_task_spawn_cmd("pwm_out_sim", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 1000, (px4_main_t)&PWMSim::task_main_trampoline, nullptr); @@ -406,7 +406,7 @@ PWMSim::task_main() ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ - if (_mixers != nullptr) { + if (_armed && _mixers != nullptr) { /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9fad2978ca..c1acccfaa1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -354,7 +354,7 @@ PX4FMU::init() _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1600, + 1200, (main_t)&PX4FMU::task_main_trampoline, nullptr); @@ -607,7 +607,7 @@ PX4FMU::task_main() orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif /* initialize PWM limit lib */ @@ -772,8 +772,8 @@ PX4FMU::task_main() // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + if (rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } for (uint8_t i = 0; i < rc_in.channel_count; i++) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cdfce1e9c8..e53bdbd0a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -672,8 +672,8 @@ PX4IO::init() return -1; } - if (_max_rc_input > RC_INPUT_MAX_CHANNELS) - _max_rc_input = RC_INPUT_MAX_CHANNELS; + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) + _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); @@ -1675,10 +1675,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ - input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); - uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + uint16_t regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; /* * Read the channel count and the first 9 channels. @@ -1697,8 +1697,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) channel_count = regs[PX4IO_P_RAW_RC_COUNT]; /* limit the channel count */ - if (channel_count > RC_INPUT_MAX_CHANNELS) { - channel_count = RC_INPUT_MAX_CHANNELS; + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } _rc_chan_count = channel_count; @@ -1736,7 +1736,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) } /* get RSSI from input channel */ - if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / ((_rssi_pwm_max - _rssi_pwm_min) / 100); rssi = rssi > 100 ? 100 : rssi; @@ -1764,19 +1764,19 @@ PX4IO::io_publish_raw_rc() /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; /* only keep publishing RC input if we ever got a valid input */ if (_rc_last_valid == 0) { @@ -2660,19 +2660,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) /* sort out the source of the values */ if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; } /* read raw R/C input values */ @@ -2757,7 +2757,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= RC_INPUT_MAX_CHANNELS) { + if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) { /* fail with error */ return -E2BIG; } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 7440a07eb4..9154c7e5fc 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c index 2c95a17330..9d9bf5d0c0 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-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 @@ -35,6 +34,8 @@ /** * @file px4_mavlink_debug.c * Debug application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index a1876265da..ea54574bb9 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-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 @@ -35,6 +34,8 @@ /** * @file px4_simple_app.c * Minimal application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 95ccf4130f..7bf4129e48 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate() float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { float airspeed_result = airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index c80eb357cf..d04fe38ac3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -60,62 +60,24 @@ ECL_PitchController::~ECL_PitchController() float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { - float roll = ctl_data.roll; /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.airspeed))) { + if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } - /* flying inverted (wings upside down) ? */ - bool inverted = false; - - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (roll > 0.0f) { - /* right hemisphere */ - roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - /* calculate the offset in the rate resulting from rolling */ - //xxx needs explanation and conversion to body angular rates or should be removed - float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll)) * _roll_ff; - - if (inverted) { - turn_offset = -turn_offset; - } - /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; - /* add turn offset */ - _rate_setpoint += turn_offset; - - /* limit the rate */ //XXX: move to body angluar rates - + /* limit the rate */ if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_rate_setpoint > 0.0f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; @@ -132,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -160,11 +122,48 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; + /* apply turning offset to desired bodyrate setpoint*/ + /* flying inverted (wings upside down)*/ + bool inverted = false; + float constrained_roll; + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); - _rate_error = _bodyrate_setpoint - pitch_bodyrate; + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + inverted = true; + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (ctl_data.roll > 0.0f) { + /* right hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + + } else { + /* left hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + + /* input conditioning */ + float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); + + /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. + For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 + Availible on google books 8/11/2015: + https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ + float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(constrained_roll) * sinf(constrained_roll))); + + if (inverted) { + body_fixed_turn_offset = -body_fixed_turn_offset; + } + + /* Finally add the turn offset to your bodyrate setpoint*/ + _bodyrate_setpoint += body_fixed_turn_offset; + + + _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1c43e7ebf7..9d40c73f3b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController() float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { + if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch) && - isfinite(ctl_data.roll_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* Transform setpoint to body angular rates (jacobian) */ _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4dd409283a..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.speed_body_u) && - isfinite(ctl_data.speed_body_v) && - isfinite(ctl_data.speed_body_w) && - isfinite(ctl_data.roll_rate_setpoint) && - isfinite(ctl_data.pitch_rate_setpoint))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.speed_body_u) && + PX4_ISFINITE(ctl_data.speed_body_v) && + PX4_ISFINITE(ctl_data.speed_body_w) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control // counter++; - if (!isfinite(_rate_setpoint)) { + if (!PX4_ISFINITE(_rate_setpoint)) { warnx("yaw rate sepoint not finite"); _rate_setpoint = 0.0f; } @@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl /* input conditioning */ float airspeed = ctl_data.airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); @@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl _bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); } - /* Transform estimation to body angular rates (jacobian) */ - float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { diff --git a/src/lib/eigen b/src/lib/eigen index e7850ed81f..240599e148 160000 --- a/src/lib/eigen +++ b/src/lib/eigen @@ -1 +1 @@ -Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 +Subproject commit 240599e1488da540c2ae8753dea0907f6cfac8ee diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6f049d4f52..ec2d64ad83 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -62,6 +62,10 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; + + // Reset the filter state as we just switched from non-altitude control + // to altitude control mode + _states_initalized = false; } _update_50hz_last_usec = now; @@ -94,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< // Only required if airspeed is being measured and controlled float temp = 0; - if (isfinite(airspeed) && airspeed_sensor_enabled()) { + if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { // Get DCM // Calculate speed rate of change // XXX check @@ -138,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, // Get airspeed or default to halfway between min and max if // airspeed is not being used and set speed rate to zero - if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { // If no airspeed available use average of min and max _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); @@ -224,12 +228,12 @@ void TECS::_update_speed_demand(void) void TECS::_update_height_demand(float demand, float state) { // Handle initialization - if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { + if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { _hgt_dem_in_old = demand; } // Apply 2 point moving average to demanded height // This is required because height demand is updated in steps - if (isfinite(demand)) { + if (PX4_ISFINITE(demand)) { _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); } else { _hgt_dem = _hgt_dem_in_old; @@ -309,12 +313,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); - float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + _STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement - STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; - _STEdotErrLast = STEdot_error; + _STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = _STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle @@ -342,7 +346,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later - _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle @@ -438,11 +442,11 @@ void TECS::_update_pitch(void) // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); - float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded - float integ7_input = SEB_error * _integGain; + float integ7_input = _SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); @@ -460,7 +464,7 @@ void TECS::_update_pitch(void) // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); - float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; if (_climbOutDem) { temp += _PITCHminf * gainInv; @@ -598,18 +602,26 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.mode = ECL_TECS_MODE_NORMAL; } - _tecs_state.hgt_dem = _hgt_dem_adj; - _tecs_state.hgt = _integ3_state; - _tecs_state.dhgt_dem = _hgt_rate_dem; - _tecs_state.dhgt = _integ2_state; - _tecs_state.spd_dem = _TAS_dem_adj; - _tecs_state.spd = _integ5_state; - _tecs_state.dspd = _vel_dot; - _tecs_state.ithr = _integ6_state; - _tecs_state.iptch = _integ7_state; - _tecs_state.thr = _throttle_dem; - _tecs_state.ptch = _pitch_dem; - _tecs_state.dspd_dem = _TAS_rate_dem; + _tecs_state.altitude_sp = _hgt_dem_adj; + _tecs_state.altitude_filtered = _integ3_state; + _tecs_state.altitude_rate_sp = _hgt_rate_dem; + _tecs_state.altitude_rate = _integ2_state; + + _tecs_state.airspeed_sp = _TAS_dem_adj; + _tecs_state.airspeed_rate_sp = _TAS_rate_dem; + _tecs_state.airspeed_filtered = _integ5_state; + _tecs_state.airspeed_rate = _vel_dot; + + _tecs_state.total_energy_error = _STE_error; + _tecs_state.energy_distribution_error = _SEB_error; + _tecs_state.total_energy_rate_error = _STEdot_error; + _tecs_state.energy_distribution_error = _SEBdot_error; + + _tecs_state.energy_error_integ = _integ6_state; + _tecs_state.energy_distribution_error_integ = _integ7_state; + + _tecs_state.throttle_integ = _integ6_state; + _tecs_state.pitch_integ = _integ7_state; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 81a5e2d691..2599e7a152 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -79,6 +79,10 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), + _STE_error(0.0f), + _STEdot_error(0.0f), + _SEB_error(0.0f), + _SEBdot_error(0.0f), _airspeed_enabled(false), _states_initalized(false), _in_air(false), @@ -137,18 +141,22 @@ public: struct tecs_state { uint64_t timestamp; - float hgt; - float dhgt; - float hgt_dem; - float dhgt_dem; - float spd_dem; - float spd; - float dspd; - float ithr; - float iptch; - float thr; - float ptch; - float dspd_dem; + float altitude_filtered; + float altitude_sp; + float altitude_rate; + float altitude_rate_sp; + float airspeed_filtered; + float airspeed_sp; + float airspeed_rate; + float airspeed_rate_sp; + float energy_error_integ; + float energy_distribution_error_integ; + float total_energy_error; + float total_energy_rate_error; + float energy_distribution_error; + float energy_distribution_rate_error; + float throttle_integ; + float pitch_integ; enum ECL_TECS_MODE mode; }; @@ -376,6 +384,15 @@ private: // Specific energy error quantities float _STE_error; + // Energy error rate + float _STEdot_error; + + // Specific energy balance error + float _SEB_error; + + // Specific energy balance error rate + float _SEBdot_error; + // Time since last update of main TECS loop (seconds) float _DT; diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9f34bc81d5..b81f8868ec 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -285,7 +285,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + res.data[i][j] = data[i][j] / num; return res; } @@ -375,7 +375,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", (double)data[i][j]); printf(" ]\n"); } diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h index 6c4556c5f5..2cd98e59aa 100644 --- a/src/lib/px4_eigen.h +++ b/src/lib/px4_eigen.h @@ -46,8 +46,6 @@ #pragma GCC diagnostic ignored "-Wfloat-equal" #define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 -#define EIGEN_MAX_STATIC_ALIGN_BYTES 16 - #include #include #pragma GCC diagnostic pop diff --git a/src/lib/uavcan b/src/lib/uavcan index 988e404586..3ae5400aa5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 988e404586900754e7e1748c87c0ec12b757cf87 +Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fb69c7086..397083a3b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -108,6 +108,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -238,6 +239,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma void set_control_mode(); +bool stabilization_required(); + void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); @@ -294,7 +297,7 @@ int commander_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 40, 3400, commander_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + (char * const *)&argv[0]); unsigned constexpr max_wait_us = 1000000; unsigned constexpr max_wait_steps = 2000; @@ -359,7 +362,7 @@ int commander_main(int argc, char *argv[]) if (calib_ret) { warnx("calibration failed, exiting."); - return 0; + return 1; } else { return 0; } @@ -388,7 +391,7 @@ int commander_main(int argc, char *argv[]) int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); px4_close(mavlink_fd_local); - return 0; + return 0; } usage("unrecognized command"); @@ -874,6 +877,23 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_armed = false; + bool startup_in_hil = false; + +#ifdef __PX4_NUTTX + /* NuttX indicates 3 arguments when only 2 are present */ + argc -= 1; +#endif + + if (argc > 2) { + if (!strcmp(argv[2],"-hil")) { + startup_in_hil = true; + } else { + PX4_ERR("Argument %s not supported.", argv[2]); + PX4_ERR("COMMANDER NOT STARTED"); + thread_should_exit = true; + } + } + /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); @@ -955,7 +975,12 @@ int commander_thread_main(int argc, char *argv[]) status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - status.hil_state = vehicle_status_s::HIL_STATE_OFF; + + if(startup_in_hil) { + status.hil_state = vehicle_status_s::HIL_STATE_ON; + } else { + status.hil_state = vehicle_status_s::HIL_STATE_OFF; + } status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -2219,7 +2244,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - //fflush(stdout); counter++; int blink_state = blink_msg_state(); @@ -2537,8 +2561,8 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); - control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_rates_enabled = stabilization_required(); + control_mode.flag_control_attitude_enabled = stabilization_required(); control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; @@ -2579,8 +2603,8 @@ set_control_mode() control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = !status.in_transition_mode; + control_mode.flag_control_velocity_enabled = !status.in_transition_mode; control_mode.flag_control_termination_enabled = false; break; @@ -2689,12 +2713,12 @@ set_control_mode() !offboard_control_mode.ignore_acceleration_force; control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode; + !offboard_control_mode.ignore_position) && !status.in_transition_mode; control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode; + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode; control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; @@ -2706,6 +2730,15 @@ set_control_mode() } } +bool +stabilization_required() +{ + return (status.is_rotary_wing || // is a rotary wing, or + status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or + (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND + !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode +} + void print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp deleted file mode 100644 index 000cbf366a..0000000000 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ /dev/null @@ -1,634 +0,0 @@ -/**************************************************************************** - * - * 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 state_machine_helper.cpp - * State machine helper functions implementations - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" -#include "PreflightCheck.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -// This array defines the arming state transitions. The rows are the new state, and the columns -// are the current state. Using new state and current state you can index into the array which -// will be true for a valid transition or false for a invalid transition. In some cases even -// though the transition is marked as true additional checks must be made. See arming_state_transition -// code for those checks. -static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI -}; - -// You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", -}; - -transition_result_t -arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status - const struct safety_s *safety, ///< current safety settings - arming_state_t new_arming_state, ///< arming state requested - struct actuator_armed_s *armed, ///< current armed status - bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing - const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none -{ - // Double check that our static arrays are still valid - ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); - ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); - - transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; - bool feedback_provided = false; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_arming_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - /* - * Get sensing state if necessary - */ - int prearm_ret = OK; - - /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - prearm_ret = prearm_check(status, mavlink_fd); - } - - /* - * Perform an atomic state update - */ - //irqstate_t flags = irqsave(); - - /* enforce lockdown in HIL */ - if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { - armed->lockdown = true; - - } else { - armed->lockdown = false; - } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - - // Do not perform pre-arm checks if coming from in air restore - // Allow if vehicle_status_s::HIL_STATE_ON - if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - - // Fail transition if pre-arm check fails - if (prearm_ret) { - /* the prearm check already prints the reject reason */ - feedback_provided = true; - valid_transition = false; - - // Fail transition if we need safety switch press - } else if (safety->safety_switch_available && !safety->safety_off) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); - feedback_provided = true; - valid_transition = false; - } - - // Perform power checks only if circuit breaker is not - // engaged for these checks - if (!status->circuit_breaker_engaged_power_check) { - // Fail transition if power is not good - if (!status->condition_power_input_valid) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); - feedback_provided = true; - valid_transition = false; - } - - // Fail transition if power levels on the avionics rail - // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { - // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.75f) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; - } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } - } - } - - } - - } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { - new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - warnx("HIL STATE ON -> STANDBY"); - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - warnx("NOT ARMING: Sensors not operational."); - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - - /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ - if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); - } else { - mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); - } - feedback_provided = true; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - } - - /* end of atomic state update */ - //irqrestore(flags); - } - - if (ret == TRANSITION_DENIED) { - const char * str = "INVAL: %s - %s"; - /* only print to console here by default as this is too technical to be useful during operation */ - warnx(str, state_names[status->arming_state], state_names[new_arming_state]); - - /* print to MAVLink if we didn't provide any feedback yet */ - if (!feedback_provided) { - mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); - } - } - - return ret; -} - -bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) -{ - // System is safe if: - // 1) Not armed - // 2) Armed, but in software lockdown (HIL) - // 3) Safety switch is present AND engaged -> actuators locked - if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { - return true; - - } else { - return false; - } -} - -transition_result_t -main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if the same state is requested because conditions may have changed */ - switch (new_main_state) { - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ACRO: - ret = TRANSITION_CHANGED; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ - /* TODO: add this for fixedwing as well */ - if (!status->is_rotary_wing || - (status->condition_local_altitude_valid || - status->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ - if (status->condition_local_position_valid || - status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* need global position estimate */ - if (status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* need global position and home position */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - - /* need offboard signal */ - if (!status->offboard_control_signal_lost) { - ret = TRANSITION_CHANGED; - } - - break; - - case vehicle_status_s::MAIN_STATE_MAX: - default: - break; - } - if (ret == TRANSITION_CHANGED) { - if (status->main_state != new_main_state) { - status->main_state = new_main_state; - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - transition_result_t ret = TRANSITION_DENIED; - - if (current_status->hil_state == new_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - switch (new_state) { - case vehicle_status_s::HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - ret = TRANSITION_DENIED; - break; - - case vehicle_status_s::HIL_STATE_ON: - if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - const char *devname; - unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; - - /* skip mavlink */ - if (!strcmp(MAVLINK_LOG_DEVICE, devname)) { - continue; - } - - - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); - ret = TRANSITION_DENIED; - } - break; - - default: - warnx("Unknown HIL state"); - break; - } - } - - if (ret == TRANSITION_CHANGED) { - current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - // XXX also set lockdown here - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - } - return ret; -} - -/** - * Check failsafe and main status and set navigation status for navigator accordingly - */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) -{ - navigation_state_t nav_state_old = status->nav_state; - - bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); - status->failsafe = false; - - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ALTCTL: - case vehicle_status_s::MAIN_STATE_POSCTL: - /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - } else { - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; - break; - - case vehicle_status_s::MAIN_STATE_MANUAL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - break; - - default: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - } - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - - /* go into failsafe - * - if commanded to do so - * - if we have an engine failure - * - depending on datalink, RC and if the mission is finished */ - - /* first look at the commands */ - if (status->engine_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status->data_link_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->gps_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->rc_signal_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - - /* finished handling commands which have priority, now handle failures */ - } else if (status->gps_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - - /* datalink loss enabled: - * check for datalink lost: this should always trigger RTGS */ - } else if (data_link_loss_enabled && status->data_link_lost) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* datalink loss disabled: - * check if both, RC and datalink are lost during the mission - * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ - } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || - (status->rc_signal_lost && mission_finished))) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ - } else if (!stay_in_failsafe){ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure */ - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* also go into failsafe if just datalink is lost */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* go into failsafe if RC is lost and datalink loss is not set up */ - } else if (status->rc_signal_lost && !data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* don't bother if RC is lost if datalink is connected */ - } else if (status->rc_signal_lost) { - - /* this mode is ok, we don't need RC for loitering */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } else { - /* everything is perfect */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* require global position and home, also go into failsafe on an engine failure */ - - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((!status->condition_global_position_valid || - !status->condition_home_position_valid)) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - /* require offboard control, otherwise stay where you are */ - if (status->offboard_control_signal_lost && !status->rc_signal_lost) { - status->failsafe = true; - - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; - } - default: - break; - } - - return status->nav_state != nav_state_old; -} - -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) -{ - /* at this point this equals the preflight check, but might add additional - * quantities later. - */ - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - checkAirspeed = true; - } - - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); -} diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index ad8df4970b..7ea93b87e9 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -39,6 +39,7 @@ #ifndef _DATAMANAGER_H #define _DATAMANAGER_H +#include #include #include diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ba33d72b19..2cf33d9b0b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -947,9 +947,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; - if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + if (!_local_pos.xy_global || + !_local_pos.v_xy_valid || + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; + } else { _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; @@ -1129,8 +1134,8 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 7500, + SCHED_PRIORITY_MAX - 40, + 4800, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index f519621137..2a582ebbb8 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_22states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c3f83e21ba..b6265ffc06 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -42,6 +42,9 @@ */ #include +#include +#include +#include #include #include #include @@ -168,7 +171,6 @@ private: float p_rmax_pos; float p_rmax_neg; float p_integrator_max; - float p_roll_feedforward; float r_p; float r_i; float r_ff; @@ -178,7 +180,6 @@ private: float y_i; float y_d; float y_ff; - float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; int32_t y_coordinated_method; @@ -211,7 +212,6 @@ private: param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; - param_t p_roll_feedforward; param_t r_p; param_t r_i; param_t r_ff; @@ -221,7 +221,6 @@ private: param_t y_i; param_t y_d; param_t y_ff; - param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; param_t y_coordinated_method; @@ -374,7 +373,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); - _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); @@ -426,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -450,7 +448,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); @@ -493,7 +490,6 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); @@ -671,8 +667,9 @@ FixedwingAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -807,7 +804,7 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (isfinite(_manual.flaps)) { + if (PX4_ISFINITE(_manual.flaps)) { flaps_control = _manual.flaps; } @@ -817,7 +814,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { @@ -997,7 +994,7 @@ FixedwingAttitudeControl::task_main() control_input.lock_integrator = lock_integrator; /* Run attitude controllers */ - if (isfinite(roll_sp) && isfinite(pitch_sp)) { + if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude @@ -1009,8 +1006,8 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; - if (!isfinite(roll_u)) { + _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1020,8 +1017,8 @@ FixedwingAttitudeControl::task_main() } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; - if (!isfinite(pitch_u)) { + _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1041,11 +1038,11 @@ FixedwingAttitudeControl::task_main() } float yaw_u = _yaw_ctrl.control_bodyrate(control_input); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; - if (!isfinite(yaw_u)) { + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1055,11 +1052,11 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ - _actuators.control[3] = (isfinite(throttle_sp) && + _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; - if (!isfinite(throttle_sp)) { + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } @@ -1139,7 +1136,6 @@ FixedwingAttitudeControl::task_main() _control_task = -1; _task_running = false; - _exit(0); } int @@ -1152,7 +1148,7 @@ FixedwingAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1166,23 +1162,29 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_att_control {start|stop|status}"); + warnx("usage: fw_att_control {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) - errx(1, "already running"); + if (att_control::g_control != nullptr) { + warnx("already running"); + return 1; + } att_control::g_control = new FixedwingAttitudeControl; - if (att_control::g_control == nullptr) - errx(1, "alloc failed"); + if (att_control::g_control == nullptr) { + warnx("alloc failed"); + return 1; + } if (OK != att_control::g_control->start()) { delete att_control::g_control; att_control::g_control = nullptr; - err(1, "start failed"); + warn("start failed"); + return 1; } /* check if the waiting is necessary at all */ @@ -1196,24 +1198,28 @@ int fw_att_control_main(int argc, char *argv[]) } printf("\n"); } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) - errx(1, "not running"); + if (att_control::g_control == nullptr){ + warnx("not running"); + return 1; + } delete att_control::g_control; att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (att_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index ed026c02eb..5e640fcc12 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -128,17 +128,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); */ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); -/** - * Roll to Pitch feedforward gain. - * - * This compensates during turns and ensures the nose stays level. - * - * @min 0.0 - * @max 2.0 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); - /** * Roll rate proportional Gain * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8094da6011..73c2bb07ac 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -52,6 +52,9 @@ */ #include +#include +#include +#include #include #include #include @@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (!isfinite(global_pos.terrain_alt)) { + if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; } @@ -1024,7 +1027,11 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) _althold_epv = _global_pos.epv; _was_in_deadband = true; } - + if (_vehicle_status.is_vtol) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { + _hold_alt = _global_pos.alt; + } + } return climbout_mode; } @@ -1728,8 +1735,9 @@ FixedwingPositionControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1787,11 +1795,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) { + if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) { + } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } @@ -1821,7 +1829,6 @@ FixedwingPositionControl::task_main() warnx("exiting.\n"); _control_task = -1; - _exit(0); } void FixedwingPositionControl::reset_takeoff_state() @@ -1920,22 +1927,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ break; } - t.altitudeSp = s.hgt_dem; - t.altitude_filtered = s.hgt; - t.airspeedSp = s.spd_dem; - t.airspeed_filtered = s.spd; + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; - t.flightPathAngleSp = s.dhgt_dem; - t.flightPathAngle = s.dhgt; - t.flightPathAngleFiltered = s.dhgt; + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; - t.airspeedDerivativeSp = s.dspd_dem; - t.airspeedDerivative = s.dspd; + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; - t.totalEnergyRateSp = s.thr; - t.totalEnergyRate = s.ithr; - t.energyDistributionRateSp = s.ptch; - t.energyDistributionRate = s.iptch; + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_integ = s.throttle_integ; + t.pitch_integ = s.pitch_integ; if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); @@ -1955,7 +1965,7 @@ FixedwingPositionControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingPositionControl::task_main_trampoline, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1969,16 +1979,20 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + warnx("usage: fw_pos_control_l1 {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (l1_control::g_control != nullptr) - errx(1, "already running"); + if (l1_control::g_control != nullptr) { + warnx("already running"); + return 1; + } if (OK != FixedwingPositionControl::start()) { - err(1, "start failed"); + warn("start failed"); + return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1989,24 +2003,28 @@ int fw_pos_control_l1_main(int argc, char *argv[]) } printf("\n"); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr) - errx(1, "not running"); + if (l1_control::g_control == nullptr){ + warnx("not running"); + return 1; + } delete l1_control::g_control; l1_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index cfba54d452..c07956fd6e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; - _status.flightPathAngleFiltered = flightPathAngleFiltered; - _status.airspeedDerivativeSp = airspeedDerivativeSp; - _status.airspeedDerivative = airspeedDerivative; - _status.totalEnergyRateSp = totalEnergyRateSp; - _status.totalEnergyRate = totalEnergyRate; - _status.energyDistributionRateSp = energyDistributionRateSp; - _status.energyDistributionRate = energyDistributionRate; - _status.mode = mode; + // /* Write part of the status message */ + // _status.flightPathAngleSp = flightPathAngleSp; + // _status.flightPathAngle = flightPathAngle; + // _status.flightPathAngleFiltered = flightPathAngleFiltered; + // _status.airspeedDerivativeSp = airspeedDerivativeSp; + // _status.airspeedDerivative = airspeedDerivative; + // _status.totalEnergyRateSp = totalEnergyRateSp; + // _status.totalEnergyRate = totalEnergyRate; + // _status.energyDistributionRateSp = energyDistributionRateSp; + // _status.energyDistributionRate = energyDistributionRate; + // _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4378e05acf..6fa5918fc5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1411,7 +1411,7 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; -#ifndef __PX4_POSIX +#ifdef __PX4_NUTTX /* the NuttX optarg handler does not * ignore argv[0] like the POSIX handler * does, nor does it deal with non-flag @@ -1484,6 +1484,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(optarg, "config") == 0) { + _mode = MAVLINK_MODE_CONFIG; } break; @@ -1600,8 +1602,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _task_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1693,6 +1693,27 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VTOL_STATE", 0.5f); break; + case MAVLINK_MODE_CONFIG: + // Enable a number of interesting streams we want via USB + configure_stream("PARAM_VALUE", 300.0f); + configure_stream("MISSION_ITEM", 50.0f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("VFR_HUD", 20.0f); + configure_stream("ATTITUDE", 100.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("MANUAL_CONTROL", 5.0f); + configure_stream("HIGHRES_IMU", 100.0f); + configure_stream("GPS_RAW_INT", 20.0f); + configure_stream("CAMERA_TRIGGER", 500.0f); + configure_stream("VTOL_STATE", 2.0f); + default: break; } @@ -1877,6 +1898,9 @@ Mavlink::task_main(int argc, char *argv[]) } perf_end(_loop_perf); + + /* confirm task running only once fully initialized */ + _task_running = true; } delete _subscribe_to_stream; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7b74fb8f09..00716480cf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -147,7 +147,8 @@ public: MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, - MAVLINK_MODE_OSD + MAVLINK_MODE_OSD, + MAVLINK_MODE_CONFIG }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 3cf0323a05..2c1a3d88f6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 559cf2c89c..414b7ee96f 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -159,9 +159,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; - strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ - _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 55c4597677..24d0940477 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1050,7 +1050,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; - rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; /* channels */ @@ -1094,7 +1094,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; - rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; /* roll */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 84f7439a58..dc98db02f3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -993,9 +993,12 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } - // XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition - if (_vehicle_status.in_transition_mode) { - reset_yaw_sp = true; + /* reset yaw and altitude setpoint for VTOL which are in fw mode */ + if (_vehicle_status.is_vtol) { + if (!_vehicle_status.is_rotary_wing) { + reset_yaw_sp = true; + _reset_alt_sp = true; + } } //Update previous arming state diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7925809f12..9f26d05016 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index b0130a1f5c..0c344bff92 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 0ca69f0f75..e6097c87f3 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index de34797aa3..e4068cac38 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -624,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s { /* select onboard/offboard mission */ int *mission_index_ptr; - struct mission_s *mission; dm_item_t dm_item; - int mission_index_next; + + struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; + int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + + /* do not work on empty missions */ + if (mission->count == 0) { + return false; + } + + /* move to next item if there is one */ + if (mission_index_next < ((int)mission->count - 1)) { + mission_index_next++; + } if (onboard) { /* onboard mission */ - mission_index_next = _current_onboard_mission_index + 1; mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; - - mission = &_onboard_mission; - dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_next = _current_offboard_mission_index + 1; mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; - - mission = &_offboard_mission; - dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -653,7 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -665,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_fd(), + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index c850a0313c..eb17a329ee 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -43,6 +43,8 @@ #include +#include + #include #include #include diff --git a/src/modules/uORB/topics/mission.h b/src/modules/navigator/navigation.h similarity index 89% rename from src/modules/uORB/topics/mission.h rename to src/modules/navigator/navigation.h index 7363e7d968..d23a20c5ff 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/navigator/navigation.h @@ -32,19 +32,18 @@ ****************************************************************************/ /** - * @file mission.h + * @file navigation.h * Definition of a mission consisting of mission items. * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier */ -#ifndef TOPIC_MISSION_H_ -#define TOPIC_MISSION_H_ +#ifndef NAVIGATION_H_ +#define NAVIGATION_H_ #include #include -#include "../uORB.h" #define NUM_MISSIONS_SUPPORTED 256 @@ -103,15 +102,7 @@ struct mission_item_s { int actuator_value; /**< new value for selected actuator in ms 900...2000 */ }; -/** - * This topic used to notify navigator about mission changes, mission itself and new mission state - * must be stored in dataman before publication. - */ -struct mission_s { - int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the dataman */ - int current_seq; /**< default -1, start at the one changed latest */ -}; +#include /** * @} diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 093e1be3c2..1ddedddff4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -45,6 +45,7 @@ #include #include +#include #include #include diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1d252ea646..d63f5fe338 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -524,7 +524,7 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX -5, + SCHED_PRIORITY_DEFAULT - 5, 1500, (px4_main_t)&Navigator::task_main_trampoline, nullptr); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 40bf7f0223..fcc00148f8 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b75b7fa221..246007ad81 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 5928f1f07b..3506065802 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,8 +44,7 @@ #include #include -#include -#include +#include #include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 05687d0943..892e50f3a1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,10 +116,10 @@ static inline int max(int val1, int val2) static void usage(const char *reason) { if (reason) { - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); } - fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n"); return; } @@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (isfinite(sensor.baro_alt_meter)) { + if (PX4_ISFINITE(sensor.baro_alt_meter)) { baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fea4f45bfc..c6d13f8756 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -106,6 +106,7 @@ #include #include #include +#include #include #include @@ -200,7 +201,8 @@ static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; /* GPS time, used for log files naming */ -static uint64_t gps_time = 0; +static uint64_t gps_time_sec = 0; +static bool has_gps_3d_fix = false; /* current state of logging */ static bool logging_enabled = false; @@ -412,13 +414,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec; - if (_gpstime_only) { - utc_time_sec = gps_time / 1e6; + if (_gpstime_only && has_gps_3d_fix) { + utc_time_sec = gps_time_sec; } else { utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); } - if (utc_time_sec > PX4_EPOCH_SECS) { + if (utc_time_sec > PX4_EPOCH_SECS) { /* strip the time elapsed since boot */ if (boot_time) { utc_time_sec -= hrt_absolute_time() / 1e6; @@ -737,9 +739,16 @@ void sdlog2_start_log() } /* write all performance counters */ + hrt_abstime curr_time = hrt_absolute_time(); + struct print_load_s load; int perf_fd = open_perf_file("preflight"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); perf_print_all(perf_fd); + dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n"); + usleep(500 * 1000); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* reset performance counters to get in-flight min and max values in post flight log */ @@ -775,8 +784,15 @@ void sdlog2_stop_log() /* write all performance counters */ int perf_fd = open_perf_file("postflight"); + hrt_abstime curr_time = hrt_absolute_time(); dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); perf_print_all(perf_fd); + struct print_load_s load; + dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); + sleep(1); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* free log writer performance counter */ @@ -989,7 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_usage(NULL); } - gps_time = 0; + gps_time_sec = 0; /* interpret logging params */ @@ -1289,7 +1305,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1320,7 +1336,8 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; + has_gps_3d_fix = buf_gps_pos.fix_type == 3; } if (!logging_enabled) { @@ -1900,15 +1917,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; - log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; - log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; - log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; + log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; + log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; + log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } @@ -1976,7 +1994,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - warnx("time: gps: %u seconds", (unsigned)gps_time); + warnx("time: gps: %u seconds", (unsigned)gps_time_sec); if (!logging_enabled) { warnx("not logging"); } else { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2874d7affb..7f77a58e7b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -363,16 +363,16 @@ struct log_TECS_s { float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; - float flightPathAngleFiltered; float airspeedSp; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; + float totalEnergyError; + float totalEnergyRateError; + float energyDistributionError; + float energyDistributionRateError; + float pitch_integ; + float throttle_integ; uint8_t mode; }; @@ -545,7 +545,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd297a95ee..eba4d7077c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1887,7 +1887,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_ROLL, 0); /** * Pitch control channel mapping. @@ -1900,7 +1900,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 0); /** * Failsafe channel mapping. @@ -1927,7 +1927,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); /** * Yaw control channel mapping. @@ -1940,7 +1940,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_YAW, 4); +PARAM_DEFINE_INT32(RC_MAP_YAW, 0); /** * Mode switch channel mapping. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d8ca75e806..bf30b6bd05 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -165,7 +165,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -239,7 +239,6 @@ private: int32_t _mag2_prio; /**< mag2 sensor priority */ int32_t _baro_prio; /**< baro0 sensor priority */ int32_t _baro1_prio; /**< baro1 sensor priority */ - int32_t _diff_pres_prio; /**< baro1 sensor priority */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -249,7 +248,7 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; - float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ + float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ @@ -294,7 +293,7 @@ private: int rc_map_aux4; int rc_map_aux5; - int rc_map_param[RC_PARAM_MAP_NCHAN]; + int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; int32_t rc_fails_thr; float rc_assist_th; @@ -350,8 +349,8 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_map_param[RC_PARAM_MAP_NCHAN]; - param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound to a RC channel, equivalent float values in the _parameters struct are not existing because these parameters are never read. */ @@ -603,9 +602,9 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - char name[PARAM_ID_LEN]; - snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -783,7 +782,7 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } @@ -831,7 +830,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } @@ -1544,6 +1543,12 @@ Sensors::parameter_update_poll(bool forced) /* run through all mag sensors */ for (unsigned s = 0; s < 3; s++) { + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); @@ -1554,12 +1559,6 @@ Sensors::parameter_update_poll(bool forced) continue; } - /* set a valid default rotation (same as board). - * if the mag is configured, this might be replaced - * in the section below. - */ - _mag_rotation[s] = _board_rotation; - bool config_ok = false; /* run through all stored calibrations */ @@ -1696,7 +1695,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink @@ -1709,17 +1708,17 @@ Sensors::rc_parameter_map_poll(bool forced) _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { - _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); } } warnx("rc to parameter map updated"); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", i, - _rc_parameter_map.param_id[i], + &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], (double)_rc_parameter_map.scale[i], (double)_rc_parameter_map.value0[i], (double)_rc_parameter_map.value_min[i], @@ -1911,7 +1910,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) void Sensors::set_params_from_rc() { - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink @@ -2142,7 +2141,7 @@ Sensors::task_main() warnx("Sensor initialization failed"); _sensors_task = -1; if (_fd_adc >=0) { - close(_fd_adc); + px4_close(_fd_adc); _fd_adc = -1; } return; diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 097903d21f..054a1c3501 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety * - * Setting this parameter to 894281 will disable IO safety. + * Setting this parameter to 22027 will disable IO safety. * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 067cf7a91b..3232adf3e7 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,11 +55,14 @@ SRCS = \ ifeq ($(PX4_TARGET_OS),nuttx) SRCS += err.c \ + printload.c \ up_cxxinitialize.c +else +SRCS += print_load_posix.c endif ifneq ($(PX4_TARGET_OS),qurt) -SRCS += hx_stream.c +SRCS += hx_stream.c endif MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 1344accb84..b0a0b01d03 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle) void perf_print_counter(perf_counter_t handle) { - perf_print_counter_fd(0, handle); + perf_print_counter_fd(1, handle); } void diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c new file mode 100644 index 0000000000..5e12dc3323 --- /dev/null +++ b/src/modules/systemlib/print_load_posix.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file print_load_posix.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ +} + diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c new file mode 100644 index 0000000000..23dde1ce44 --- /dev/null +++ b/src/modules/systemlib/printload.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file printload.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +static const char * +tstate_name(const tstate_t s) +{ + switch (s) { + case TSTATE_TASK_INVALID: + return "init"; + + case TSTATE_TASK_PENDING: + return "PEND"; + + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; +#ifndef CONFIG_DISABLE_SIGNALS + + case TSTATE_WAIT_SIG: + return "w:sig"; +#endif +#ifndef CONFIG_DISABLE_MQUEUE + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; +#endif +#ifdef CONFIG_PAGING + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; +#endif + + default: + return "ERROR"; + } +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ + print_state->new_time = t; + + int i; + uint64_t curr_time_us; + uint64_t idle_time_us; + char *clear_line = ""; + + /* print system information */ + if (fd == 1) { + dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + clear_line = CL; + } + + curr_time_us = t; + idle_time_us = system_load.tasks[0].total_runtime; + + if (print_state->new_time > print_state->interval_start_time) { + print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000)); + } + + print_state->running_count = 0; + print_state->blocked_count = 0; + print_state->total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime; + + if (system_load.tasks[i].valid) { + switch (system_load.tasks[i].tcb->task_state) { + case TSTATE_TASK_PENDING: + case TSTATE_TASK_READYTORUN: + case TSTATE_TASK_RUNNING: + print_state->running_count++; + break; + + case TSTATE_TASK_INVALID: + case TSTATE_TASK_INACTIVE: + case TSTATE_WAIT_SEM: +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: + case TSTATE_WAIT_MQNOTFULL: +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: +#endif + print_state->blocked_count++; + break; + } + } + + interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 && + system_load.tasks[i].total_runtime > print_state->last_times[i]) + ? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000 + : 0; + + print_state->last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv; + + if (i > 0) { + print_state->total_user_time += interval_runtime; + } + + } else { + print_state->curr_loads[i] = 0; + } + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle; + float task_load; + float sched_load; + + idle = print_state->curr_loads[0]; + task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv; + + /* this can happen if one tasks total runtime was not computed + correctly by the scheduler instrumentation TODO */ + if (task_load > (1.f - idle)) { + task_load = (1.f - idle); + } + + sched_load = 1.f - idle - task_load; + + dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n", + clear_line, + system_load.total_count, + print_state->running_count, + print_state->blocked_count); + dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", + clear_line, + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); + dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n", + clear_line, + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d, + clear_line); + + /* header for task list */ + dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n", + clear_line, + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", +#if CONFIG_RR_INTERVAL > 0 + "TSLICE" +#else + "STATE" +#endif + ); + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) { + break; + } + + stack_free++; + } + + dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", + clear_line, + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(print_state->curr_loads[i] * 100.0f), + (int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); + +#if CONFIG_RR_INTERVAL > 0 + /* print scheduling info with RR time slice */ + dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice); +#else + // print task state instead + dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); +#endif + } + } + + print_state->interval_start_time = print_state->new_time; +} + diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/systemlib/printload.h similarity index 56% rename from src/modules/uORB/topics/rc_parameter_map.h rename to src/modules/systemlib/printload.h index 6e68dc4b6f..ee210f8903 100644 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ b/src/modules/systemlib/printload.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -20,7 +20,7 @@ * "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 , + * 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 @@ -32,45 +32,39 @@ ****************************************************************************/ /** - * @file rc_parameter_map.h - * Maps RC channels to parameters + * @file printload.h * - * @author Thomas Gubler + * Print the current system load. + * + * @author Lorenz Meier */ -#ifndef TOPIC_RC_PARAMETER_MAP_H -#define TOPIC_RC_PARAMETER_MAP_H +#pragma once +__BEGIN_DECLS + +#include #include -#include "../uORB.h" -#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h -#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN +#ifndef CONFIG_MAX_TASKS +#define CONFIG_MAX_TASKS 64 +#endif -/** - * @addtogroup topics - * @{ - */ +struct print_load_s { + uint64_t total_user_time; -struct rc_parameter_map_s { - uint64_t timestamp; /**< time at which the map was updated */ + int running_count; + int blocked_count; - bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ - - int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this - this field is ignored if set to -1, in this case param_id will - be used*/ - char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ - float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ - float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ - float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ - float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ + uint64_t new_time; + uint64_t interval_start_time; + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + float interval_time_ms_inv; }; -/** - * @} - */ +__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); -ORB_DECLARE(rc_parameter_map); +__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); -#endif +__END_DECLS diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 6a71fc25f4..b483c00a80 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm) SRCS += uORBTest_UnitTest.cpp endif -ifneq ($(PX4_TARGET_OS),qurt) -SRCS += Publication.cpp \ - Subscription.cpp -endif - SRCS += objects_common.cpp \ uORBUtils.cpp \ uORB.cpp \ - uORBMain.cpp + uORBMain.cpp \ + Publication.cpp \ + Subscription.cpp MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index df56e931ca..ac98048771 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -45,23 +45,23 @@ #include -#include -ORB_DEFINE(sensor_mag, struct mag_report); +#include "topics/sensor_mag.h" +ORB_DEFINE(sensor_mag, struct sensor_mag_s); -#include -ORB_DEFINE(sensor_accel, struct accel_report); +#include "topics/sensor_accel.h" +ORB_DEFINE(sensor_accel, struct sensor_accel_s); -#include -ORB_DEFINE(sensor_gyro, struct gyro_report); +#include "topics/sensor_gyro.h" +ORB_DEFINE(sensor_gyro, struct sensor_gyro_s); -#include -ORB_DEFINE(sensor_baro, struct baro_report); +#include "topics/sensor_baro.h" +ORB_DEFINE(sensor_baro, struct sensor_baro_s); -#include -ORB_DEFINE(output_pwm, struct pwm_output_values); +#include "topics/output_pwm.h" +ORB_DEFINE(output_pwm, struct output_pwm_s); -#include -ORB_DEFINE(input_rc, struct rc_input_values); +#include "topics/input_rc.h" +ORB_DEFINE(input_rc, struct input_rc_s); #include "topics/pwm_input.h" ORB_DEFINE(pwm_input, struct pwm_input_s); @@ -140,6 +140,10 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" +ORB_DEFINE(mission, struct mission_s); +// XXX onboard and offboard mission are still declared here until this is +// generator supported +#include ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 3ae0f56a30..7b40cff219 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -338,7 +338,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_priority(int handle, int *priority) __EXPORT; +extern int orb_priority(int handle, int32_t *priority) __EXPORT; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index f9dbb5ad03..15f2f5c854 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -68,8 +68,8 @@ int UavcanEscController::init() { // ESC status subscription int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("ESC status sub failed %i", res); return res; } @@ -83,9 +83,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || - (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Rate limiting - we don't want to congest the bus */ const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } + _prev_cmd_pub = timestamp; /* @@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - // trim negative values back to 0. Previously - // we set this to 0.1, which meant motors kept - // spinning when armed, but that should be a - // policy decision for a specific vehicle - // type, as it is not appropriate for all - // types of vehicles (eg. fixed wing). + + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). if (scaled < 0.0F) { scaled = 0.0F; - } + } + if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); @@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); + } else { msg.cmd.push_back(static_cast(0)); } @@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm) { if (arm) { _armed_mask = -1; + } else { _armed_mask = 0; } @@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm) { if (arm) { _armed_mask = MOTOR_BIT(num); + } else { _armed_mask = 0; } @@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< } } -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) { _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; if (_esc_status_pub != nullptr) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index a0af493b59..77f0f1d610 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -54,7 +54,7 @@ class UavcanEscController { public: - UavcanEscController(uavcan::INode& node); + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); int init(); @@ -79,12 +79,12 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; - typedef uavcan::MethodBinder&)> StatusCbBinder; - typedef uavcan::MethodBinder - TimerCbBinder; + typedef uavcan::MethodBinder + TimerCbBinder; bool _armed = false; esc_status_s _esc_status = {}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1d6c205586..da62e55634 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ + uavcan_servers.cpp \ uavcan_clock.cpp \ uavcan_params.c @@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) \ +-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ +-DUAVCAN_NO_ASSERTIONS \ +-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 98ff1e5a72..1986de4247 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -45,9 +45,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : _sub_air_pressure_data(node), _sub_air_temperature_data(node), _reports(nullptr) -{ - last_temperature = 0.0f; -} +{ } int UavcanBarometerBridge::init() { @@ -153,7 +151,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { - last_temperature = msg.static_temperature; + last_temperature_kelvin = msg.static_temperature; } void UavcanBarometerBridge::air_pressure_sub_cb(const @@ -162,7 +160,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = last_temperature; + report.temperature = last_temperature_kelvin - 273.15F; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index ef08d367fa..38cf6ab89a 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -78,6 +78,6 @@ private: uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; ringbuffer::RingBuffer *_reports; - float last_temperature; + float last_temperature_kelvin = 0.0f; }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index c67719c29e..0830aba4ee 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -46,20 +46,21 @@ const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : -_node(node), -_sub_fix(node), -_report_pub(nullptr) + _node(node), + _sub_fix(node), + _report_pub(nullptr) { } int UavcanGnssBridge::init() { int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; } @@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const void UavcanGnssBridge::print_status() const { printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { printf("N/A\n"); + } else { printf("%d\n", _receiver_node_id); } @@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { report.eph = -1.0F; report.epv = -1.0F; } if (valid_velocity_covariance) { - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. @@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure(arg), sizeof(_scale)); - return 0; - } + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { - std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); - return 0; - } + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { - return 0; // Nothing to do - } + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { - return 1; // declare it external rise it's priority and to allow for correct orientation compensation - } + return 1; // declare it external rise it's priority and to allow for correct orientation compensation + } + case MAGIOCSSAMPLERATE: { - return 0; // Pretend that this stuff is supported to keep the sensor app happy - } + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: case MAGIOCGSAMPLERATE: case MAGIOCSRANGE: @@ -121,23 +132,25 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar case MAGIOCSLOWPASS: case MAGIOCEXSTRAP: case MAGIOCGLOWPASS: { - return -EINVAL; - } + return -EINVAL; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure + &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; unlock(); publish(msg.getSrcNodeID().get(), &_report); diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index b74afeb0be..a13883a494 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -40,7 +40,7 @@ #include "sensor_bridge.hpp" #include -#include +#include class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { @@ -57,14 +57,14 @@ private: ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, void (UavcanMagnetometerBridge::*) - (const uavcan::ReceivedDataStructure &) > + (const uavcan::ReceivedDataStructure &) > MagCbBinder; - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; mag_report _report = {}; }; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index fa97b3c89e..9dc89c74d5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,7 +45,7 @@ /* * IUavcanSensorBridge */ -void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { list.add(new UavcanBarometerBridge(node)); list.add(new UavcanMagnetometerBridge(node)); @@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } + delete [] _channels; } @@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; DEVICE_LOG("out of class instances"); @@ -128,6 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); } + assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); @@ -136,11 +139,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id >= 0) { out += 1; } } + return out; } @@ -152,6 +157,7 @@ void UavcanCDevSensorBridgeBase::print_status() const if (_channels[i].node_id >= 0) { printf("channel %d: node id %d --> class instance %d\n", i, _channels[i].node_id, _channels[i].class_instance); + } else { printf("channel %d: empty\n", i); } diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 8faad4e4be..7138ec337b 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -45,7 +45,7 @@ /** * A sensor bridge class must implement this interface. */ -class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: static constexpr unsigned MAX_NAME_LEN = 20; @@ -77,7 +77,7 @@ public: * Sensor bridge factory. * Creates all known sensor bridges and puts them in the linked list. */ - static void make_all(uavcan::INode &node, List &list); + static void make_all(uavcan::INode &node, List &list); }; /** @@ -86,8 +86,7 @@ public: */ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev { - struct Channel - { + struct Channel { int node_id = -1; orb_advert_t orb_advert = nullptr; int class_instance = -1; @@ -104,13 +103,13 @@ protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor, - const unsigned max_channels = DEFAULT_MAX_CHANNELS) : - device::CDev(name, devname), - _max_channels(max_channels), - _class_devname(class_devname), - _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]) + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : + device::CDev(name, devname), + _max_channels(max_channels), + _class_devname(class_devname), + _orb_topic(orb_topic_sensor), + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 63ccea49bc..ed3ba5e445 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,15 +53,12 @@ #include #include "uavcan_main.hpp" +#include -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 -#endif + /** * @file uavcan_main.cpp @@ -76,33 +73,28 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; -#if defined(USE_FW_NODE_SERVER) -uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; -uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; -uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; -uavcan_posix::FirmwareVersionChecker fw_version_checker; -#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), -#if !defined(USE_FW_NODE_SERVER) _esc_controller(_node) -#else - _esc_controller(_node), - _fileserver_backend(_node), - _node_info_retriever(_node), - _fw_upgrade_trigger(_node, fw_version_checker), - _fw_server(_node, _fileserver_backend) -#endif - { + _task_should_exit = false; + _fw_server_action = None; + _fw_server_status = -1; + _tx_injector = nullptr; _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - const int res = pthread_mutex_init(&_node_mutex, nullptr); + int res = pthread_mutex_init(&_node_mutex, nullptr); + + if (res < 0) { + std::abort(); + } + + res = sem_init(&_server_command_sem, 0 , 0); if (res < 0) { std::abort(); @@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys UavcanNode::~UavcanNode() { + + fw_server(Stop); + if (_task != -1) { + /* tell the task we want it to go away */ _task_should_exit = true; @@ -161,13 +157,127 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - -#if defined(USE_FW_NODE_SERVER) - delete(_server_instance); -#endif + pthread_mutex_destroy(&_node_mutex); + sem_destroy(&_server_command_sem); } +int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) +{ + int rv = -1; + + if (UavcanNode::instance()) { + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + rv = 0; + } + + return rv; +} + +int UavcanNode::start_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers == nullptr) { + + rv = UavcanServers::start(2, _node); + + if (rv >= 0) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().installRxFrameListener(driver.get()); + * would require a dynamic cast and rtti is not enabled. + */ + UavcanServers::instance()->attachITxQueueInjector(&_tx_injector); + } + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + +int UavcanNode::request_fw_check() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + _servers->requestCheckAllNodesFirmwareAndUpdate(); + rv = 0; + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; + +} + +int UavcanNode::stop_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + _tx_injector = nullptr; + + rv = _servers->stop(); + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + + +int UavcanNode::fw_server(eServerAction action) +{ + int rv = -EAGAIN; + + switch (action) { + case Start: + case Stop: + case CheckFW: + if (_fw_server_action == None) { + _fw_server_action = action; + sem_wait(&_server_command_sem); + rv = _fw_server_status; + } + + break; + + default: + rv = -EINVAL; + break; + } + + return rv; +} + + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; + } + const int node_init_res = _instance->init(node_id); if (node_init_res < 0) { @@ -248,7 +363,7 @@ void UavcanNode::fill_node_info() assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); @@ -256,21 +371,7 @@ void UavcanNode::fill_node_info() /* hardware version */ uavcan::protocol::HardwareVersion hwver; - - if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { - hwver.major = 1; - - } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { - hwver.major = 2; - - } else { - ; // All other values of HW_ARCH resolve to zero - } - - uint8_t udid[12] = {}; // Someone seems to love magic numbers - get_board_serial(udid); - uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); - + getHardwareVersion(hwver); _node.setHardwareVersion(hwver); } @@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } -#if defined(USE_FW_NODE_SERVER) - /* Initialize the fw version checker. - * giving it it's path - */ - ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); - - if (ret < 0) { - return ret; - } - - - /* Start fw file server back */ - - ret = _fw_server.start(); - - if (ret < 0) { - return ret; - } - - /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ - - ret = storage_backend.init(UAVCAN_NODE_DB_PATH); - - if (ret < 0) { - return ret; - } - - /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ - - ret = tracer.init(UAVCAN_LOG_FILE); - - if (ret < 0) { - return ret; - } - - /* Create dynamic node id server for the Firmware updates directory */ - - _server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer); - - if (_server_instance == 0) { - return -ENOMEM; - } - - /* Initialize the dynamic node id server */ - ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id); - - if (ret < 0) { - return ret; - } - - /* Start node info retriever to fetch node info from new nodes */ - - ret = _node_info_retriever.start(); - - if (ret < 0) { - return ret; - } - - - /* Start the fw version checker */ - - ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath()); - - if (ret < 0) { - return ret; - } - -#endif /* Start the Node */ return _node.start(); @@ -398,6 +431,11 @@ void UavcanNode::node_spin_once() warnx("node spin error %i", spin_res); } + + if (_tx_injector != nullptr) { + _tx_injector->injectTxFramesInto(_node); + } + perf_end(_perfcnt_node_spin_elapsed); } @@ -446,7 +484,7 @@ int UavcanNode::run() * IO multiplexing shall be done here. */ - _node.setStatusOk(); + _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). @@ -466,6 +504,24 @@ int UavcanNode::run() while (!_task_should_exit) { + switch (_fw_server_action) { + case Start: + _fw_server_status = start_fw_server(); + break; + + case Stop: + _fw_server_status = stop_fw_server(); + break; + + case CheckFW: + _fw_server_status = request_fw_check(); + break; + + case None: + default: + break; + } + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { + sem_post(&_server_command_sem); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); @@ -812,7 +870,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop|arm|disarm}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + bool fw = argc > 2 && !std::strcmp(argv[2], "fw"); + if (!std::strcmp(argv[1], "start")) { if (UavcanNode::instance()) { + if (fw && UavcanServers::instance() == nullptr) { + int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); + + if (rv < 0) { + warnx("Firmware Server Failed to Start %d", rv); + ::exit(rv); + } + + ::exit(0); + } + // Already running, no error warnx("already started"); ::exit(0); @@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[]) errx(1, "application not running"); } + if (fw && !std::strcmp(argv[1], "update")) { + if (UavcanServers::instance() == nullptr) { + errx(1, "firmware server is not running"); + } + + int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW); + ::exit(rv); + } + + if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) { + printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped"); + ::exit(0); + } + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); ::exit(0); @@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + if (fw) { + + int rv = inst->fw_server(UavcanNode::Stop); + + if (rv < 0) { + warnx("Firmware Server Failed to Stop %d", rv); + ::exit(rv); + } + + ::exit(0); + + } else { + delete inst; + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b7222f9d47..acc1c32843 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -48,13 +48,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include -# include -# include -#endif +# include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -65,10 +59,6 @@ */ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 -#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" -#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db" -#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw" -#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) @@ -84,8 +74,7 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -99,11 +88,12 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 3400; + static constexpr unsigned StackSize = 1600; public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; + enum eServerAction {None, Start, Stop, CheckFW , Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -126,6 +116,9 @@ public: void print_info(); static UavcanNode *instance() { return _instance; } + static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); + int fw_server(eServerAction action); + void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} private: void fill_node_info(); @@ -133,10 +126,14 @@ private: void node_spin_once(); int run(); int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] - + int start_fw_server(); + int stop_fw_server(); + int request_fw_check(); int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + volatile eServerAction _fw_server_action; + int _fw_server_status; int _armed_sub = -1; ///< uORB subscription of the arming status actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus @@ -151,22 +148,13 @@ private: Node _node; ///< library instance pthread_mutex_t _node_mutex; - + sem_t _server_command_sem; UavcanEscController _esc_controller; - -#if defined(USE_FW_NODE_SERVER) - static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer - - uavcan_posix::BasicFileSeverBackend _fileserver_backend; - uavcan::NodeInfoRetriever _node_info_retriever; - uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; - uavcan::BasicFileServer _fw_server; -#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; - + ITxQueueInjector *_tx_injector; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index a74c92dc55..3a49bd3eb7 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -41,10 +41,13 @@ /** * Enable UAVCAN. * - * Enables support for UAVCAN-interfaced actuators and sensors. + * Allowed values: + * 0 - UAVCAN disabled. + * 1 - Enabled support for UAVCAN actuators and sensors. + * 2 - Enabled support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 1 + * @max 2 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp new file mode 100644 index 0000000000..1713b282e7 --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uavcan_main.hpp" +#include "uavcan_servers.hpp" +#include "uavcan_virtual_can_driver.hpp" + +# include +# include +# include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +# define OK 0 + +/** + * @file uavcan_servers.cpp + * + * Implements basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * David Sidrane + */ + +/* + * UavcanNode + */ +UavcanServers *UavcanServers::_instance; +UavcanServers::UavcanServers(uavcan::INode &main_node) : + _subnode_thread(-1), + _vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _main_node(main_node), + _tracer(), + _storage_backend(), + _fw_version_checker(), + _server_instance(_subnode, _storage_backend, _tracer), + _fileserver_backend(_subnode), + _node_info_retriever(_subnode), + _fw_upgrade_trigger(_subnode, _fw_version_checker), + _fw_server(_subnode, _fileserver_backend), + _mutex_inited(false), + _check_fw(false) + +{ +} + +UavcanServers::~UavcanServers() +{ + if (_mutex_inited) { + (void)Lock::deinit(_subnode_mutex); + } + _main_node.getDispatcher().removeRxFrameListener(); +} + +int UavcanServers::stop(void) +{ + UavcanServers *server = instance(); + + if (server == nullptr) { + warnx("Already stopped"); + return -1; + } + + _instance = nullptr; + + if (server->_subnode_thread != -1) { + pthread_cancel(server->_subnode_thread); + pthread_join(server->_subnode_thread, NULL); + } + + server->_main_node.getDispatcher().removeRxFrameListener(); + + delete server; + return 0; +} + +int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) +{ + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } + + /* + * Node init + */ + _instance = new UavcanServers(main_node); + + if (_instance == nullptr) { + warnx("Out of memory"); + return -2; + } + + int rv = _instance->init(num_ifaces); + + if (rv < 0) { + warnx("Node init failed: %d", rv); + delete _instance; + _instance = nullptr; + return rv; + } + + /* + * Start the thread. Normally it should never exit. + */ + pthread_attr_t tattr; + struct sched_param param; + + pthread_attr_init(&tattr); + tattr.stacksize = StackSize; + param.sched_priority = Priority; + pthread_attr_setschedparam(&tattr, ¶m); + + static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; + + rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); + + if (rv < 0) { + warnx("pthread_create() failed: %d", errno); + rv = -errno; + delete _instance; + _instance = nullptr; + } + + return rv; +} + +int UavcanServers::init(unsigned num_ifaces) +{ + errno = 0; + + /* + * Initialize the mutex. + * giving it its path + */ + + int ret = Lock::init(_subnode_mutex); + + if (ret < 0) { + warnx("Lock init: %d", errno); + return ret; + } + + _mutex_inited = true; + + _subnode.setNodeID(_main_node.getNodeID()); + _main_node.getDispatcher().installRxFrameListener(&_vdriver); + + + /* + * Initialize the fw version checker. + * giving it its path + */ + ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); + + if (ret < 0) { + warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno); + return ret; + } + + /* Start fw file server back */ + + ret = _fw_server.start(); + + if (ret < 0) { + warnx("BasicFileServer init: %d, errno: %d", ret, errno); + return ret; + } + + /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ + + ret = _storage_backend.init(UAVCAN_NODE_DB_PATH); + + if (ret < 0) { + warnx("FileStorageBackend init: %d, errno: %d", ret, errno); + return ret; + } + + /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ + + ret = _tracer.init(UAVCAN_LOG_FILE); + + if (ret < 0) { + warnx("FileEventTracer init: %d, errno: %d", ret, errno); + return ret; + } + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + UavcanNode::getHardwareVersion(hwver); + + /* Initialize the dynamic node id server */ + ret = _server_instance.init(hwver.unique_id); + + if (ret < 0) { + warnx("CentralizedServer init: %d", ret); + return ret; + } + + /* Start node info retriever to fetch node info from new nodes */ + + ret = _node_info_retriever.start(); + + if (ret < 0) { + warnx("NodeInfoRetriever init: %d", ret); + return ret; + } + + /* Start the fw version checker */ + + ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); + + if (ret < 0) { + warnx("FirmwareUpdateTrigger init: %d", ret); + return ret; + } + + /* Start the Node */ + + return OK; +} +__attribute__((optimize("-O0"))) +pthread_addr_t UavcanServers::run(pthread_addr_t) +{ + Lock lock(_subnode_mutex); + + while (1) { + + if (_check_fw == true) { + _check_fw = false; + _node_info_retriever.invalidateAll(); + } + + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } + } + + warnx("exiting."); + return (pthread_addr_t) 0; +} + diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp new file mode 100644 index 0000000000..9a68a8cc25 --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +#include +#include + +# include +# include +# include +# include +# include +# include +# include +# include + +# include "uavcan_virtual_can_driver.hpp" + +/** + * @file uavcan_servers.hpp + * + * Defines basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * @author David Sidrane + */ + +#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" + +/** + * A UAVCAN Server Sub node. + */ +class UavcanServers +{ + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; + + static constexpr unsigned MaxCanFramsPerTransfer = 63; + + /** + * This number is based on the worst case max number of frames per interface. With + * MemPoolBlockSize set at 48 this is 6048 Bytes. + * + * The servers can be forced to use the primary interface only, this can be achieved simply by passing + * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. + */ + static constexpr unsigned QueuePoolSize = + (UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); + + static constexpr unsigned StackSize = 3500; + static constexpr unsigned Priority = 120; + + typedef uavcan::SubNode SubNode; + +public: + UavcanServers(uavcan::INode &main_node); + + virtual ~UavcanServers(); + + static int start(unsigned num_ifaces, uavcan::INode &main_node); + static int stop(void); + + SubNode &get_node() { return _subnode; } + + static UavcanServers *instance() { return _instance; } + + /* + * Set the main node's pointer to to the injector + * This is a work around as main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;} + + void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; } + +private: + pthread_t _subnode_thread; + pthread_mutex_t _subnode_mutex; + + int init(unsigned num_ifaces); + void deinit(void); + + pthread_addr_t run(pthread_addr_t); + + static UavcanServers *_instance; ///< singleton pointer + + typedef VirtualCanDriver vCanDriver; + + vCanDriver _vdriver; + + uavcan::SubNode _subnode; ///< library instance + uavcan::INode &_main_node; ///< library instance + + uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; + uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; + uavcan_posix::FirmwareVersionChecker _fw_version_checker; + uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + + bool _mutex_inited; + volatile bool _check_fw; + +}; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp new file mode 100644 index 0000000000..5ef9de0d87 --- /dev/null +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Pavel Kirienko + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* + * General purpose wrapper around os's mutual exclusion + * mechanism. + * + * It supports the + * + * Note the naming of thier_mutex_ implies that the underlying + * mutex is owned by class using the Lock. + * and this wrapper provides service to initialize and de initialize + * the mutex. + */ +class Lock +{ + pthread_mutex_t &thier_mutex_; + +public: + + Lock(pthread_mutex_t &m) : + thier_mutex_(m) + { + (void)pthread_mutex_lock(&m); + } + + ~Lock() + { + (void)pthread_mutex_unlock(&thier_mutex_); + } + + static int init(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_init(&thier_mutex_, NULL); + } + + static int deinit(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_destroy(&thier_mutex_); + } + +}; + +/** + * Generic queue based on the linked list class defined in libuavcan. + * This class does not use heap memory. + */ +template +class Queue +{ + struct Item : public uavcan::LinkedListNode { + T payload; + + template + Item(Args... args) : payload(args...) { } + }; + + uavcan::LimitedPoolAllocator allocator_; + uavcan::LinkedListRoot list_; + +public: + Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : + allocator_(arg_allocator, block_allocation_quota) + { + uavcan::IsDynamicallyAllocatable::check(); + } + + bool isEmpty() const { return list_.isEmpty(); } + + /** + * Creates one item in-place at the end of the list. + * Returns true if the item was appended successfully, false if there's not enough memory. + * Complexity is O(N) where N is queue length. + */ + template + bool tryEmplace(Args... args) + { + // Allocating memory + void *const ptr = allocator_.allocate(sizeof(Item)); + + if (ptr == nullptr) { + return false; + } + + // Constructing the new item + Item *const item = new(ptr) Item(args...); + assert(item != nullptr); + + // Inserting the new item at the end of the list + Item *p = list_.get(); + + if (p == nullptr) { + list_.insert(item); + + } else { + while (p->getNextListNode() != nullptr) { + p = p->getNextListNode(); + } + + assert(p->getNextListNode() == nullptr); + p->setNextListNode(item); + assert(p->getNextListNode()->getNextListNode() == nullptr); + } + + return true; + } + + /** + * Accesses the first element. + * Nullptr will be returned if the queue is empty. + * Complexity is O(1). + */ + T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; } + const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; } + + /** + * Removes the first element. + * If the queue is empty, nothing will be done and assertion failure will be triggered. + * Complexity is O(1). + */ + void pop() + { + Item *const item = list_.get(); + assert(item != nullptr); + + if (item != nullptr) { + list_.remove(item); + item->~Item(); + allocator_.deallocate(item); + } + } +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + */ +class VirtualCanIface : public uavcan::ICanIface, + uavcan::Noncopyable +{ + /** + * This class re-defines uavcan::RxCanFrame with flags. + * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded, + * rendering this class unusable with Queue<>. + */ + struct RxItem: public uavcan::CanFrame { + const uavcan::MonotonicTime ts_mono; + const uavcan::UtcTime ts_utc; + const uavcan::CanIOFlags flags; + const uint8_t iface_index; + + RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) : + uavcan::CanFrame(arg_frame), + ts_mono(arg_frame.ts_mono), + ts_utc(arg_frame.ts_utc), + flags(arg_flags), + iface_index(arg_frame.iface_index) + { + // Making sure it will fit into a pool block with a pointer prefix + static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee"); + } + }; + + pthread_mutex_t &common_driver_mutex_; + + uavcan::CanTxQueue prioritized_tx_queue_; + Queue rx_queue_; + + int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override + { + Lock lock(common_driver_mutex_); + prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags); + return 1; + } + + int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override + { + Lock lock(common_driver_mutex_); + + if (rx_queue_.isEmpty()) { + return 0; + } + + const auto item = *rx_queue_.peek(); + rx_queue_.pop(); + + out_frame = item; + out_ts_monotonic = item.ts_mono; + out_ts_utc = item.ts_utc; + out_flags = item.flags; + + return 1; + } + + int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; } + uint16_t getNumFilters() const override { return 0; } + uint64_t getErrorCount() const override { return 0; } + +public: + VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock, + pthread_mutex_t &arg_mutex, unsigned quota_per_queue) : + common_driver_mutex_(arg_mutex), + prioritized_tx_queue_(allocator, clock, quota_per_queue), + rx_queue_(allocator, quota_per_queue) + { + } + + ~VirtualCanIface() + { + } + + /** + * Note that RX queue overwrites oldest items when overflowed. + * Call this from the main thread only. + * No additional locking is required. + */ + void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) + { + Lock lock(common_driver_mutex_); + + if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) { + rx_queue_.pop(); + (void)rx_queue_.tryEmplace(frame, flags); + } + } + + /** + * Call this from the main thread only. + * No additional locking is required. + */ + void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index) + { + Lock lock(common_driver_mutex_); + const std::uint8_t iface_mask = static_cast(1U << iface_index); + + while (auto e = prioritized_tx_queue_.peek()) { + UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s", + unsigned(iface_mask), e->toString().c_str()); + + const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask, + uavcan::CanTxQueue::Qos(e->qos), e->flags); + + prioritized_tx_queue_.remove(e); + + if (res <= 0) { + break; + } + + } + } + + /** + * Call this from the sub-node thread only. + * No additional locking is required. + */ + bool hasDataInRxQueue() + { + Lock lock(common_driver_mutex_); + return !rx_queue_.isEmpty(); + } +}; + +/** + * This interface defines one method that will be called by the main node thread periodically in order to + * transfer contents of TX queue of the sub-node into the TX queue of the main node. + */ +class ITxQueueInjector +{ +public: + virtual ~ITxQueueInjector() { } + + /** + * Flush contents of TX queues into the main node. + * @param main_node Reference to the main node. + */ + virtual void injectTxFramesInto(uavcan::INode &main_node) = 0; +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the + * memory pool that will be shared across all interfaces for RX/TX queues. + * Typically this value should be no less than 4K per interface. + */ +template +class VirtualCanDriver : public uavcan::ICanDriver, + public uavcan::IRxFrameListener, + public ITxQueueInjector, + uavcan::Noncopyable +{ + class Event + { + FAR sem_t sem; + + + public: + + int init() + { + return sem_init(&sem, 0, 0); + } + + int deinit() + { + return sem_destroy(&sem); + } + + + Event() + { + } + + ~Event() + { + } + + + /** + */ + + void waitFor(uavcan::MonotonicDuration duration) + { + static const int NsPerSec = 1000000000; + + if (duration.isPositive()) { + auto abstime = ::timespec(); + + if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) { + abstime.tv_nsec += duration.toUSec() * 1000; + + if (abstime.tv_nsec >= NsPerSec) { + abstime.tv_sec++; + abstime.tv_nsec -= NsPerSec; + } + + (void)sem_timedwait(&sem, &abstime); + } + } + } + + void signal() + { + int count; + int rv = sem_getvalue(&sem, &count); + + if (rv > 0 && count <= 0) { + sem_post(&sem); + } + } + }; + + Event event_; ///< Used to unblock the select() call when IO happens. + pthread_mutex_t driver_mutex_; ///< Shared across all ifaces + uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; + const unsigned num_ifaces_; + uavcan::ISystemClock &clock_; + + uavcan::ICanIface *getIface(uint8_t iface_index) override + { + return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr; + } + + uint8_t getNumIfaces() const override { return num_ifaces_; } + + /** + * This and other methods of ICanDriver will be invoked by the sub-node thread. + */ + int16_t select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline) override + { + bool need_block = (inout_masks.write == 0); // Write queue is infinite + + for (unsigned i = 0; need_block && (i < num_ifaces_); i++) { + const bool need_read = inout_masks.read & (1U << i); + + if (need_read && ifaces_[i]->hasDataInRxQueue()) { + need_block = false; + } + } + + if (need_block) { + event_.waitFor(blocking_deadline - clock_.getMonotonic()); + } + + inout_masks = uavcan::CanSelectMasks(); + + for (unsigned i = 0; i < num_ifaces_; i++) { + const std::uint8_t iface_mask = 1U << i; + inout_masks.write |= iface_mask; // Always ready to write + + if (ifaces_[i]->hasDataInRxQueue()) { + inout_masks.read |= iface_mask; + } + } + + return num_ifaces_; // We're always ready to write, hence > 0. + } + + /** + * This handler will be invoked by the main node thread. + */ + void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override + { + UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str()); + + if (frame.iface_index < num_ifaces_) { + ifaces_[frame.iface_index]->addRxFrame(frame, flags); + event_.signal(); + + } + } + + /** + * This method will be invoked by the main node thread. + */ + void injectTxFramesInto(uavcan::INode &main_node) override + { + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i]->flushTxQueueTo(main_node, i); + } + + event_.signal(); + } + +public: + VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + num_ifaces_(arg_num_ifaces), + clock_(system_clock) + { + Lock::init(driver_mutex_); + event_.init(); + + assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); + + const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; + const unsigned quota_per_queue = quota_per_iface; // 2x overcommit + + UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", + unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + } + } + + ~VirtualCanDriver() + { + Lock::deinit(driver_mutex_); + event_.deinit(); + } + +}; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ea9c688f2d..a50079b14b 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -342,10 +342,14 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; + } _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw _actuators_out_1->control[4] = _tilt_control; diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk index 472dc7dba6..0e5f463e69 100644 --- a/src/platforms/common/module.mk +++ b/src/platforms/common/module.mk @@ -2,6 +2,5 @@ # Common OS porting APIs # -SRCS = px4_getopt.c \ - px4_log.c +SRCS = px4_getopt.c diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c deleted file mode 100644 index a2c61ab297..0000000000 --- a/src/platforms/common/px4_log.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; - -__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk index 1738063a56..93b55e67ab 100644 --- a/src/platforms/nuttx/px4_layer/module.mk +++ b/src/platforms/nuttx/px4_layer/module.mk @@ -35,6 +35,7 @@ # NuttX porting layer # -SRCS = px4_nuttx_tasks.c +SRCS = px4_nuttx_tasks.c \ + ../../posix/px4_layer/px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index f4e398d186..9f914ed693 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -96,6 +96,11 @@ int main(int argc, char **argv) if (argc == 2) { ifstream infile(argv[1]); + if (!infile) { + cout << "failed opening script" << argv[1] << std::endl; + return 1; + } + for (string line; getline(infile, line, '\n'); ) { process_line(line); } diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 7d7e75afee..846ce9469c 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -40,5 +40,6 @@ SRCS = \ px4_posix_tasks.cpp \ lib_crc32.c \ drv_hrt.c \ + px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c new file mode 100644 index 0000000000..238b324194 --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -0,0 +1,5 @@ +#include + +__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; + +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index ab6e0f287c..d071893b0c 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -38,6 +38,24 @@ #pragma once +#define _PX4_LOG_LEVEL_ALWAYS 0 +#define _PX4_LOG_LEVEL_DEBUG 1 +#define _PX4_LOG_LEVEL_WARN 2 +#define _PX4_LOG_LEVEL_ERROR 3 +#define _PX4_LOG_LEVEL_PANIC 4 + +// Used to silence unused variable warning +static inline void do_nothing(int level, ...) +{ + (void)level; +} + +/**************************************************************************** + * __px4_log_omit: + * Compile out the message + ****************************************************************************/ +#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) + #if defined(__PX4_ROS) #include @@ -47,6 +65,54 @@ #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) #define PX4_DEBUG(...) +#elif defined(__PX4_QURT) +#include "qurt_log.h" +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#endif +#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) + #else #define __STDC_FORMAT_MACROS @@ -59,18 +125,6 @@ __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); -// Used to silence unused variable warning -static inline void do_nothing(int level, ...) -{ - (void)level; -} - -#define _PX4_LOG_LEVEL_ALWAYS 0 -#define _PX4_LOG_LEVEL_PANIC 1 -#define _PX4_LOG_LEVEL_ERROR 2 -#define _PX4_LOG_LEVEL_WARN 3 -#define _PX4_LOG_LEVEL_DEBUG 4 - __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; @@ -107,11 +161,22 @@ __EXPORT extern int __px4_log_level_current; * Use these to implement the code level macros below ****************************************************************************/ + /**************************************************************************** - * __px4_log_omit: - * Compile out the message + * __px4_log_named_cond: + * Convert a message in the form: + * PX4_LOG_COND(__dbg_enabled, "val is %d", val); + * to + * printf("%-5s val is %d\n", "LOG", val); + * if the first arg/condition is true. ****************************************************************************/ -#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) +#define __px4_log_named_cond(name, cond, FMT, ...) \ + __px4__log_startcond(cond)\ + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + __px4__log_endline /**************************************************************************** * __px4_log_named_cond: diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 79162e3f6b..76e1218ba5 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -47,6 +47,10 @@ #include #include +#ifdef __PX4_QURT + #include +#endif + __BEGIN_DECLS #define HPWORK 0 diff --git a/src/platforms/qurt/eagle_px4_firmware_patch.patch b/src/platforms/qurt/eagle_px4_firmware_patch.patch new file mode 100644 index 0000000000..41ac284b51 --- /dev/null +++ b/src/platforms/qurt/eagle_px4_firmware_patch.patch @@ -0,0 +1,190 @@ +diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h +index 4ba2f98..a7b664e 100644 +--- a/src/include/mavlink/mavlink_log.h ++++ b/src/include/mavlink/mavlink_log.h +@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink critical message and print to console. +@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__) ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink emergency message and print to console. +@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); +- ++*/ + struct mavlink_logmessage { + char text[MAVLINK_LOG_MAXLEN + 1]; + unsigned char severity; +diff --git a/src/lib/eigen b/src/lib/eigen +--- a/src/lib/eigen ++++ b/src/lib/eigen +@@ -1 +1 @@ +-Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 ++Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty +diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp +index bbf5f8e..50c7d8b 100644 +--- a/src/modules/commander/PreflightCheck.cpp ++++ b/src/modules/commander/PreflightCheck.cpp +@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro + } + } + ++ ++#ifdef __PX4_QURT ++ // WARNING: Preflight checks are important and should be added back when ++ // all the sensors are supported ++ PX4_WARN("WARNING: Preflight checks PASS always."); ++ failed = false; ++#endif ++ + /* Report status */ + return !failed; + } +diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp +index 971e086..c165322 100644 +--- a/src/modules/commander/commander.cpp ++++ b/src/modules/commander/commander.cpp +@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[]) + } + + /* RC input check */ ++#ifndef __PX4_QURT + if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && +- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++#else ++ // HACK: remove old data check due to timestamp issue in QURT ++ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) { ++#endif + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; +@@ -2526,7 +2531,7 @@ set_control_mode() + /* set vehicle_control_mode according to set_navigation_state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); +- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; ++ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; + control_mode.flag_control_offboard_enabled = false; + + switch (status.nav_state) { +diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c +index 892e50f..7ac6776 100644 +--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c ++++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c +@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) + }; + + /* wait for initial baro value */ ++#ifdef __PX4_QURT ++ // WARNING skipping ++ PX4_WARN("Hacked to skip baro initialization for testing."); ++ bool wait_baro = false; ++#else + bool wait_baro = true; ++#endif + + thread_running = true; + +diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp +index 3fa4458..0ef1883 100644 +--- a/src/modules/sensors/sensors.cpp ++++ b/src/modules/sensors/sensors.cpp +@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + ++#ifdef __PX4_QURT ++// Temporary fix required due to lack of time sync between apps and dsp cores ++ raw.timestamp = hrt_absolute_time(); ++#else + raw.timestamp = gyro_report.timestamp; ++#endif + raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; + } +@@ -2068,6 +2073,7 @@ Sensors::task_main() + + /* start individual sensors */ + int ret = 0; ++#if 0 + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; +@@ -2091,7 +2097,7 @@ Sensors::task_main() + } + return; + } +- ++#endif + /* + * do subscriptions + */ +diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp +index 83a9378..ec86faf 100644 +--- a/src/systemcmds/mixer/mixer.cpp ++++ b/src/systemcmds/mixer/mixer.cpp +@@ -118,6 +118,8 @@ load(const char *devname, const char *fname) + return 1; + } + ++#ifndef __PX4_QURT ++ + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { + warnx("can't load mixer: %s", fname); + return 1; +@@ -125,6 +127,24 @@ load(const char *devname, const char *fname) + + /* XXX pass the buffer to the device */ + int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); ++#else ++ char newbuf[] = ++ "R: 4x 10000 10000 10000 0\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 4 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 5 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 6 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 7 10000 10000 0 -10000 10000\n"; ++ /* XXX pass the buffer to the device */ ++ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); ++#endif + + if (ret < 0) { + warnx("error loading mixers from %s", fname); diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h new file mode 100644 index 0000000000..29ef6302e1 --- /dev/null +++ b/src/platforms/qurt/include/qurt_log.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef QURT_LOG_H +#define QURT_LOG_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//void qurt_log(int level, const char *file, int line, const char *format, ...); + +// declaration to make the compiler happy. This symbol is part of the adsp static image. +void HAP_debug(const char *msg, int level, const char *filename, int line); + +static __inline void qurt_log(int level, const char *file, int line, + const char *format, ...) +{ + char buf[256]; + va_list args; + va_start(args, format); + vsnprintf(buf, sizeof(buf), format, args); + va_end(args); + HAP_debug(buf, level, file, line); +} + +#ifdef __cplusplus +} +#endif + +#endif // QURT_LOG_H + diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index b795fba341..3f6c23b732 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -48,7 +48,7 @@ const char *get_commands() "param set CAL_MAG0_ID 196608\n" // "rgbled start\n" // "tone_alarm start\n" - "commander start\n" + "commander start -hil\n" "sensors start\n" //"ekf_att_pos_estimator start\n" "attitude_estimator_q start\n" diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 916992cae4..45be19ec8a 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -46,8 +46,6 @@ static struct sq_queue_s callout_queue; -extern uint64_t get_abs_time_in_us(); - /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; @@ -83,15 +81,9 @@ static void hrt_unlock(void) */ hrt_abstime hrt_absolute_time(void) { - - return get_abs_time_in_us(); -/* struct timespec ts; - - // FIXME - clock_gettime unsupported in QuRT - //clock_gettime(CLOCK_MONOTONIC, &ts); + clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); -*/ } /* @@ -252,7 +244,7 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); @@ -273,11 +265,11 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + ticks = USEC2TICK((next->deadline - now)); } } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index da25cabcd2..f4a538a13a 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -165,11 +165,9 @@ int dspal_entry( int argc, char* argv[] ) px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for( ;; ) - { - volatile int x = 0; - ++x; - } + for( ;; ){ + usleep( 1000000 ); + } return 0; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index fcae64f8c5..9925e05dfa 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -59,9 +59,7 @@ __BEGIN_DECLS extern uint64_t get_ticks_per_us(); -// FIXME - sysconf(_SC_CLK_TCK) not supported -//long PX4_TICKS_PER_SEC = get_ticks_per_us(); -long PX4_TICKS_PER_SEC = 800000000; +long PX4_TICKS_PER_SEC = 1000; unsigned int sleep(unsigned int sec) { diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 8889f4531f..83a9378619 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -91,10 +91,10 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); - fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer load \n"); + PX4_INFO("usage:\n"); + PX4_INFO(" mixer load \n"); } static int diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index ca48a86a82..fe9614bc69 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 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 @@ -36,8 +35,8 @@ * @file top.c * Tool similar to UNIX top command * @see http://en.wikipedia.org/wiki/Top_unix - * - * @author Lorenz Meier + * + * @author Lorenz Meier */ #include @@ -49,208 +48,34 @@ #include #include +#include #include -#define CL "\033[K" // clear line - /** * Start the top application. */ -__EXPORT int top_main(void); - -extern struct system_load_s system_load; - -static const char * -tstate_name(const tstate_t s) -{ - switch (s) { - case TSTATE_TASK_INVALID: return "init"; - - case TSTATE_TASK_PENDING: return "PEND"; - case TSTATE_TASK_READYTORUN: return "READY"; - case TSTATE_TASK_RUNNING: return "RUN"; - - case TSTATE_TASK_INACTIVE: return "inact"; - case TSTATE_WAIT_SEM: return "w:sem"; -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: return "w:sig"; -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe"; - case TSTATE_WAIT_MQNOTFULL: return "w:mqf"; -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: return "w:pgf"; -#endif - - default: - return "ERROR"; - } -} +__EXPORT int top_main(int argc, char *argv[]); int -top_main(void) +top_main(int argc, char *argv[]) { - uint64_t total_user_time = 0; + hrt_abstime curr_time = hrt_absolute_time(); - int running_count = 0; - int blocked_count = 0; - - uint64_t new_time = hrt_absolute_time(); - uint64_t interval_start_time = new_time; - - uint64_t last_times[CONFIG_MAX_TASKS]; - float curr_loads[CONFIG_MAX_TASKS]; - - for (int t = 0; t < CONFIG_MAX_TASKS; t++) - last_times[t] = 0; - - float interval_time_ms_inv = 0.f; + struct print_load_s load; + init_print_load_s(curr_time, &load); /* clear screen */ - printf("\033[2J"); + dprintf(1, "\033[2J\n"); + + if (argc > 1 && !strcmp(argv[1], "once")) { + print_load(curr_time, 1, &load); + sleep(1); + print_load(hrt_absolute_time(), 1, &load); + return 0; + } for (;;) { - int i; - uint64_t curr_time_us; - uint64_t idle_time_us; - - curr_time_us = hrt_absolute_time(); - idle_time_us = system_load.tasks[0].total_runtime; - - if (new_time > interval_start_time) - interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); - - running_count = 0; - blocked_count = 0; - total_user_time = 0; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime; - - if (system_load.tasks[i].valid) { - switch (system_load.tasks[i].tcb->task_state) { - case TSTATE_TASK_PENDING: - case TSTATE_TASK_READYTORUN: - case TSTATE_TASK_RUNNING: - running_count++; - break; - - case TSTATE_TASK_INVALID: - case TSTATE_TASK_INACTIVE: - case TSTATE_WAIT_SEM: -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: - case TSTATE_WAIT_MQNOTFULL: -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: -#endif - blocked_count++; - break; - } - } - - interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; - - last_times[i] = system_load.tasks[i].total_runtime; - - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - curr_loads[i] = interval_runtime * interval_time_ms_inv; - - if (i > 0) - total_user_time += interval_runtime; - } else - curr_loads[i] = 0; - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - if (system_load.tasks[i].tcb->pid == 0) { - float idle; - float task_load; - float sched_load; - - idle = curr_loads[0]; - task_load = (float)(total_user_time) * interval_time_ms_inv; - - /* this can happen if one tasks total runtime was not computed - correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) - task_load = (1.f - idle); - - sched_load = 1.f - idle - task_load; - - /* print system information */ - printf("\033[H"); /* move cursor home and clear screen */ - printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); - printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); - printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); - - /* header for task list */ - printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", -#if CONFIG_RR_INTERVAL > 0 - "TSLICE" -#else - "STATE" -#endif - ); - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; - - stack_free++; - } - - printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); - -#if CONFIG_RR_INTERVAL > 0 - /* print scheduling info with RR time slice */ - printf(" %6d\n", system_load.tasks[i].tcb->timeslice); -#else - // print task state instead - printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); -#endif - } - } - - interval_start_time = new_time; + print_load(curr_time, 1, &load); /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { @@ -279,7 +104,7 @@ top_main(void) usleep(200000); } - new_time = hrt_absolute_time(); + curr_time = hrt_absolute_time(); } return OK; diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 99a981f521..80eb8c24e6 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -75,7 +75,7 @@ endfunction() add_library( px4_platform # ${PX_SRC}/platforms/common/px4_getopt.c - ${PX_SRC}/platforms/common/px4_log.c + ${PX_SRC}/platforms/posix/px4_layer/px4_log.c ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp ${PX_SRC}/platforms/posix/work_queue/work_lock.c