From f22c574b877070b919fbad03d2a5e0a7e846b952 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 9 Jan 2016 23:00:59 -0800 Subject: [PATCH 001/105] Integrated cmake_hexagon for qurt build Still a WIP since the IDL file needs to be replace with the muorb IDL file. Signed-off-by: Mark Charlebois --- cmake/common/px4_base.cmake | 4 +++ cmake/configs/qurt_eagle_default.cmake | 9 +++--- cmake/qurt/px4_impl_qurt.cmake | 7 ++--- src/firmware/qurt/CMakeLists.txt | 29 +++++++++---------- .../ekf_att_pos_estimator_main.cpp | 5 ++++ .../mc_att_control/mc_att_control_main.cpp | 8 +---- .../mc_pos_control/mc_pos_control_main.cpp | 6 ---- src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 6 ++-- src/platforms/qurt/px4_layer/main.cpp | 1 - .../qurt/px4_layer/px4_qurt_tasks.cpp | 1 - 10 files changed, 32 insertions(+), 44 deletions(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index fa34b1584d..dadaf9ec74 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -266,6 +266,10 @@ function(px4_add_module) add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${SRCS}) + if(${OS} STREQUAL "qurt" ) + set_property(TARGET ${MODULE} PROPERTY POSITION_INDEPENDENT_CODE TRUE) + endif() + if(MAIN) set_target_properties(${MODULE} PROPERTIES COMPILE_DEFINITIONS PX4_MAIN=${MAIN}_app_main) diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake index e39a96fe3e..b2a88be322 100644 --- a/cmake/configs/qurt_eagle_default.cmake +++ b/cmake/configs/qurt_eagle_default.cmake @@ -11,17 +11,18 @@ include(qurt/px4_impl_qurt) #include_directories(${HEXAGON_DRIVERS_ROOT}/inc) # For Actual flight we need to link against the driver dynamic libraries -set(target_libraries - -L${HEXAGON_DRIVERS_ROOT}/libs +#set(target_libraries +# -L${HEXAGON_DRIVERS_ROOT}/libs # The plan is to replace these with our drivers # mpu9x50 # uart_esc # csr_gps # rc_receiver - ) +# ) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list # diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 5c40dfebd7..0f7441c158 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -172,16 +172,13 @@ function(px4_os_add_flags) set(added_definitions -D__PX4_QURT -D__PX4_POSIX - -D__DF_QURT -include ${PX4_INCLUDE_DIR}visibility.h ) # Add the toolchain specific flags - set(added_cflags ${QURT_CMAKE_C_FLAGS}) - set(added_cxx_flags ${QURT_CMAKE_CXX_FLAGS}) + set(added_cflags -O0) + set(added_cxx_flags -O0) - # FIXME @jgoppert - how to work around issues like this? - # Without changing global variables? # Clear -rdynamic flag which fails for hexagon set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index bb55e8dc22..c16ed4f2b9 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -1,32 +1,29 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes") + px4_qurt_generate_builtin_commands( OUT ${CMAKE_BINARY_DIR}/apps.h MODULE_LIST ${module_libraries}) -# Clear -rdynamic flag which fails for hexagon -# this change is scoped in this directory and below -set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") -set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") - # Enable build without HexagonSDK to check link dependencies if ("${QURT_ENABLE_STUBS}" STREQUAL "1") add_executable(mainapp ${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c ${CMAKE_BINARY_DIR}/apps.h) else() - add_library(mainapp + QURT_BUNDLE(APP_NAME + mainapp + DSP_SOURCES ${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c - ${CMAKE_BINARY_DIR}/apps.h) + ${CMAKE_BINARY_DIR}/apps.h + DSP_LINK_LIBS + ${module_libraries} + ${target_libraries} + df_driver_framework + m + ) + endif() -target_link_libraries(mainapp - -Wl,--whole-archive - ${module_libraries} - ${target_libraries} - df_driver_framework - m - -Wl,--no-whole-archive - -Wl,${TOOLSLIB}/pic/libstdc++.a) - # vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 f9b0b1cbca..326ae229af 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 @@ -74,12 +74,17 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure + +// These are unused +#if 0 static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +#endif + static constexpr float EPH_LARGE_VALUE = 1000.0f; static constexpr float EPV_LARGE_VALUE = 1000.0f; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 067fa909ae..5461f0ec97 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -291,12 +291,6 @@ private: namespace mc_att_control { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - MulticopterAttitudeControl *g_control; } @@ -1025,4 +1019,4 @@ int mc_att_control_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; -} \ No newline at end of file +} 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 dad5173cd5..bfc368367b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -333,12 +333,6 @@ private: namespace pos_control { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - MulticopterPositionControl *g_control; } diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index 82a3301322..cd8648f08a 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -38,7 +38,6 @@ // static intialization. uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; -static hrt_abstime _check_time; static unsigned long _dropped_pkts; static unsigned long _get_min = 0xFFFFFF; static unsigned long _get_max = 0; @@ -136,7 +135,6 @@ int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t leng int16_t rc = 0; hrt_abstime t1, t2; static hrt_abstime check_time = 0; - int32_t initial_queue_size = 0; if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { //there is no-remote subscriber. So do not queue the message. @@ -280,7 +278,7 @@ int16_t uORB::FastRpcChannel::get_data static hrt_abstime check_time = 0; hrt_abstime t1 = hrt_absolute_time(); _DataAvailableSemaphore.wait(); - hrt_abstime t2 = hrt_absolute_time(); + // hrt_abstime t2 = hrt_absolute_time(); _QueueMutex.lock(); if (DataQSize() != 0 || ControlQSize() != 0) { @@ -395,7 +393,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data static hrt_abstime check_time = 0; hrt_abstime t1 = hrt_absolute_time(); _DataAvailableSemaphore.wait(); - hrt_abstime t2 = hrt_absolute_time(); + //hrt_abstime t2 = hrt_absolute_time(); _QueueMutex.lock(); diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index d8caf4659b..e9c7ff9cdf 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -102,7 +102,6 @@ static void process_commands(map &apps, const char *cmds) vector appargs; int i = 0; const char *b = cmds; - bool found_first_char = false; char arg[256]; // Eat leading whitespace diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b73e296b0c..aa89d25606 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -363,7 +363,6 @@ static void timer_cb(void *data) int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) { - void *result; work_s _hpwork = {}; // Create a timer to unblock From a8c49809face75f716621e44921bac91134190f0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 9 Jan 2016 23:16:20 -0800 Subject: [PATCH 002/105] Fixes for other qurt builds to use QURT_BUNDLE Signed-off-by: Mark Charlebois --- .gitmodules | 3 + cmake/cmake_hexagon | 1 + cmake/configs/qurt_eagle_hello.cmake | 4 +- cmake/configs/qurt_eagle_hil.cmake | 3 +- cmake/configs/qurt_eagle_muorb.cmake | 4 +- cmake/configs/qurt_eagle_release.cmake | 3 +- cmake/configs/qurt_eagle_test.cmake | 3 +- cmake/configs/qurt_eagle_travis.cmake | 3 +- .../toolchains/Toolchain-hexagon-7.2.10.cmake | 254 ------------------ cmake/toolchains/Toolchain-hexagon.cmake | 249 ----------------- src/firmware/qurt/CMakeLists.txt | 1 + src/firmware/qurt/mainapp.idl | 41 +++ src/lib/DriverFramework | 2 +- src/platforms/qurt/dspal/dspal_stub.c | 10 +- 14 files changed, 69 insertions(+), 512 deletions(-) create mode 160000 cmake/cmake_hexagon delete mode 100644 cmake/toolchains/Toolchain-hexagon-7.2.10.cmake delete mode 100644 cmake/toolchains/Toolchain-hexagon.cmake create mode 100755 src/firmware/qurt/mainapp.idl diff --git a/.gitmodules b/.gitmodules index 98a23841b5..c5188f49e9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -34,3 +34,6 @@ [submodule "src/lib/ecl"] path = src/lib/ecl url = https://github.com/PX4/ecl.git +[submodule "cmake/cmake_hexagon"] + path = cmake/cmake_hexagon + url = https://github.com/ATLFlight/cmake_hexagon diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon new file mode 160000 index 0000000000..e64ded44b2 --- /dev/null +++ b/cmake/cmake_hexagon @@ -0,0 +1 @@ +Subproject commit e64ded44b248fa8571647e11a4aeed7833ecd8a7 diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake index 4328a3d4aa..3427746a1d 100644 --- a/cmake/configs/qurt_eagle_hello.cmake +++ b/cmake/configs/qurt_eagle_hello.cmake @@ -1,6 +1,8 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) + set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 0f2e92b0c0..fb34bc4d02 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -1,6 +1,7 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake index d73d7becff..0da1fdb1a7 100644 --- a/cmake/configs/qurt_eagle_muorb.cmake +++ b/cmake/configs/qurt_eagle_muorb.cmake @@ -1,6 +1,8 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) + set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 67d84c8fa7..1e7beed49c 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -20,7 +20,8 @@ set(target_libraries ) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list # diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake index 2e6a370950..3602056e01 100644 --- a/cmake/configs/qurt_eagle_test.cmake +++ b/cmake/configs/qurt_eagle_test.cmake @@ -1,6 +1,7 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 85fae3373b..4550c504cc 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -3,7 +3,8 @@ include(qurt/px4_impl_qurt) # Run a full link with build stubs to make sure qurt target isn't broken set(QURT_ENABLE_STUBS "1") -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) +include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list drivers/device diff --git a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake deleted file mode 100644 index 1ab6fd4060..0000000000 --- a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake +++ /dev/null @@ -1,254 +0,0 @@ -# -# 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. -# - -include(CMakeForceCompiler) - -list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) -include(common/px4_base) - -if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "") - message(FATAL_ERROR - "The HexagonTools version 7.2.10 must be installed and the environment variable HEXAGON_TOOLS_ROOT must be set" - "(e.g. export HEXAGON_TOOLS_ROOT=/opt/HEXAGON_Tools/7.2.10/Tools)") -else() - set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT}) -endif() - -macro (list2string out in) - set(list ${ARGV}) - list(REMOVE_ITEM list ${out}) - foreach(item ${list}) - set(${out} "${${out}} ${item}") - endforeach() -endmacro(list2string) - -set(V_ARCH "v5") -set(CROSSDEV "hexagon-") -set(HEXAGON_BIN ${HEXAGON_TOOLS_ROOT}/bin) -set(HEXAGON_LIB_DIR ${HEXAGON_TOOLS_ROOT}/gnu/hexagon/lib) -set(HEXAGON_ISS_DIR ${HEXAGON_TOOLS_ROOT}/lib/iss) -set(TOOLSLIB ${HEXAGON_TOOLS_ROOT}/target/hexagon/lib/${V_ARCH}/G0) - -# Use the HexagonTools compiler (7.2.10) -set(CMAKE_C_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang) -set(CMAKE_CXX_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang++) - -set(CMAKE_AR ${HEXAGON_BIN}/${CROSSDEV}ar CACHE FILEPATH "Archiver") -set(CMAKE_RANLIB ${HEXAGON_BIN}/${CROSSDEV}ranlib) -set(CMAKE_LINKER ${HEXAGON_BIN}/${CROSSDEV}ld.qcld) -set(CMAKE_NM ${HEXAGON_BIN}/${CROSSDEV}nm) -set(CMAKE_OBJDUMP ${HEXAGON_BIN}/${CROSSDEV}objdump) -set(CMAKE_OBJCOPY ${HEXAGON_BIN}/${CROSSDEV}objcopy) - -list2string(HEXAGON_INCLUDE_DIRS - -I${HEXAGON_TOOLS_ROOT}/target/hexagon/include - ) - -#set(DYNAMIC_LIBS -Wl,${TOOLSLIB}/pic/libstdc++.a) - -#set(MAXOPTIMIZATION -O0) - -# Base CPU flags for each of the supported architectures. -# -set(ARCHCPUFLAGS - -m${V_ARCH} - -G0 - ) - -add_definitions( - -D_PID_T -D_UID_T -D_TIMER_T - -Dnoreturn_function= - -D_HAS_C9X - -D__EXPORT= - -Drestrict= - -D_DEBUG - -Wno-error=shadow - ) - -# optimisation flags -# -set(ARCHOPTIMIZATION - -O0 - -g - -fno-strict-aliasing - -fdata-sections - -fno-zero-initialized-in-bss - ) - -# Language-specific flags -# -set(ARCHCFLAGS - -std=gnu99 - -D__CUSTOM_FILE_IO__ - ) -set(ARCHCXXFLAGS - -fno-exceptions - -fno-rtti - -std=c++11 - -fno-threadsafe-statics - -DCONFIG_WCHAR_BUILTIN - -D__CUSTOM_FILE_IO__ - ) - -set(ARCHWARNINGS - -Wall - -Wextra - -Werror - -Wno-unused-parameter - -Wno-unused-function - -Wno-unused-variable - -Wno-gnu-array-member-paren-init - -Wno-cast-align - -Wno-missing-braces - -Wno-strict-aliasing -# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - ) - -# C-specific warnings -# -set(ARCHCWARNINGS - ${ARCHWARNINGS} - -Wstrict-prototypes - -Wnested-externs - ) - -# C++-specific warnings -# -set(ARCHWARNINGSXX - ${ARCHWARNINGS} - -Wno-missing-field-initializers - ) -exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-libgcc-file-name OUTPUT_VARIABLE LIBGCC) -exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-file-name=libm.a OUTPUT_VARIABLE LIBM) -set(EXTRA_LIBS ${EXTRA_LIBS} ${LIBM}) - -# Flags we pass to the C compiler -# -list2string(CFLAGS - ${ARCHCFLAGS} - ${ARCHCWARNINGS} - ${ARCHOPTIMIZATION} - ${ARCHCPUFLAGS} - ${ARCHINCLUDES} - ${INSTRUMENTATIONDEFINES} - ${ARCHDEFINES} - ${EXTRADEFINES} - ${EXTRACFLAGS} - ${HEXAGON_INCLUDE_DIRS} - ) - -# Flags we pass to the C++ compiler -# -list2string(CXXFLAGS - ${ARCHCXXFLAGS} - ${ARCHWARNINGSXX} - ${ARCHOPTIMIZATION} - ${ARCHCPUFLAGS} - ${ARCHXXINCLUDES} - ${INSTRUMENTATIONDEFINES} - ${ARCHDEFINES} - ${EXTRADEFINES} - ${EXTRACXXFLAGS} - ${HEXAGON_INCLUDE_DIRS} - ) - -# Flags we pass to the assembler -# -list2string(AFLAGS - ${CFLAGS} - -D__ASSEMBLY__ - ${EXTRADEFINES} - ${EXTRAAFLAGS} - ) - -# Set cmake flags -# -list2string(CMAKE_C_FLAGS - ${CMAKE_C_FLAGS} - ${CFLAGS} - ) - -set(QURT_CMAKE_C_FLAGS ${CMAKE_C_FLAGS} CACHE STRING "cflags") - -message(STATUS "CMAKE_C_FLAGS: -${CMAKE_C_FLAGS}-") - -list2string(CMAKE_CXX_FLAGS - ${CMAKE_CXX_FLAGS} - ${CXXFLAGS} - ) - -set(QURT_CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} CACHE STRING "cxxflags") - -message(STATUS "CMAKE_CXX_FLAGS: -${CMAKE_CXX_FLAGS}-") - -# Flags we pass to the linker -# -list2string(CMAKE_EXE_LINKER_FLAGS - -g - -mv5 - -mG0lib - -G0 - -fpic - -shared - -Wl,-Bsymbolic - -Wl,--wrap=malloc - -Wl,--wrap=calloc - -Wl,--wrap=free - -Wl,--wrap=realloc - -Wl,--wrap=memalign - -Wl,--wrap=__stack_chk_fail - -lc - ${EXTRALDFLAGS} - ) - -# where is the target environment -set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) - -set(CMAKE_C_COMPILER_ID, "Clang") -set(CMAKE_CXX_COMPILER_ID, "Clang") - -# search for programs in the build host directories -set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) -# for libraries and headers in the target directories -set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) -set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) - -# The Hexagon compiler doesn't support the -rdynamic flag and this is set -# in the base cmake scripts. We have to redefine the __linux_compiler_gnu -# macro for cmake 2.8 to work -set(__LINUX_COMPILER_GNU 1) -macro(__linux_compiler_gnu lang) - set(CMAKE_SHARED_LIBRARY_LINK_${lang}_FLAGS "") -endmacro() - diff --git a/cmake/toolchains/Toolchain-hexagon.cmake b/cmake/toolchains/Toolchain-hexagon.cmake deleted file mode 100644 index 4de37f553c..0000000000 --- a/cmake/toolchains/Toolchain-hexagon.cmake +++ /dev/null @@ -1,249 +0,0 @@ -# -# 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. -# - -include(CMakeForceCompiler) - -list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) -include(common/px4_base) - -if(NOT HEXAGON_TOOLS_ROOT) - set(HEXAGON_TOOLS_ROOT /opt/6.4.05) -endif() - -if(NOT HEXAGON_SDK_ROOT) - set(HEXAGON_SDK_ROOT /opt/Hexagon_SDK) -endif() - -macro (list2string out in) - set(list ${ARGV}) - list(REMOVE_ITEM list ${out}) - foreach(item ${list}) - set(${out} "${${out}} ${item}") - endforeach() -endmacro(list2string) - -set(V_ARCH "v5") -set(CROSSDEV "hexagon-") -set(HEXAGON_BIN ${HEXAGON_TOOLS_ROOT}/gnu/bin) -set(HEXAGON_CLANG_BIN ${HEXAGON_TOOLS_ROOT}/qc/bin) -set(HEXAGON_LIB_DIR ${HEXAGON_TOOLS_ROOT}/gnu/hexagon/lib) -set(HEXAGON_ISS_DIR ${HEXAGON_TOOLS_ROOT}/qc/lib/iss) -set(TOOLSLIB ${HEXAGON_TOOLS_ROOT}/dinkumware/lib/$(V_ARCH)/G0) -set(QCTOOLSLIB ${HEXAGON_TOOLS_ROOT}/qc/lib/$(V_ARCH)/G0) - -# Use the HexagonTools compiler (6.4.05) -set(CMAKE_C_COMPILER ${HEXAGON_CLANG_BIN}/${CROSSDEV}clang) -set(CMAKE_CXX_COMPILER ${HEXAGON_CLANG_BIN}/${CROSSDEV}clang++) - -set(CMAKE_AR ${HEXAGON_BIN}/${CROSSDEV}ar CACHE FILEPATH "Archiver") -set(CMAKE_RANLIB ${HEXAGON_BIN}/${CROSSDEV}ranlib) -set(CMAKE_LINKER ${HEXAGON_BIN}/${CROSSDEV}ld) -set(CMAKE_NM ${HEXAGON_BIN}/${CROSSDEV}nm) -set(CMAKE_OBJDUMP ${HEXAGON_BIN}/${CROSSDEV}objdump) -set(CMAKE_OBJCOPY ${HEXAGON_BIN}/${CROSSDEV}objcopy) - -list2string(HEXAGON_INCLUDE_DIRS - -I${HEXAGON_TOOLS_ROOT}/gnu/hexagon/include - -I${HEXAGON_SDK_ROOT}/inc - -I${HEXAGON_SDK_ROOT}/inc/stddef - ) - -#set(DYNAMIC_LIBS -Wl,${TOOLSLIB}/pic/libstdc++.a) - -#set(MAXOPTIMIZATION -O0) - -# Base CPU flags for each of the supported architectures. -# -set(ARCHCPUFLAGS - -m${V_ARCH} - -G0 - ) - -add_definitions( - -D_PID_T -D_UID_T -D_TIMER_T - -Dnoreturn_function= - -D__EXPORT= - -Drestrict= - -D_DEBUG - -Wno-error=shadow - ) - -# optimisation flags -# -set(ARCHOPTIMIZATION - -O0 - -g - -fno-strict-aliasing - -fdata-sections - -fpic - -fno-zero-initialized-in-bss - ) - -# Language-specific flags -# -set(ARCHCFLAGS - -std=gnu99 - -D__CUSTOM_FILE_IO__ - ) -set(ARCHCXXFLAGS - -fno-exceptions - -fno-rtti - -std=c++11 - -fno-threadsafe-statics - -DCONFIG_WCHAR_BUILTIN - -D__CUSTOM_FILE_IO__ - ) - -set(ARCHWARNINGS - -Wall - -Wextra - -Werror - -Wno-unused-parameter - -Wno-unused-function - -Wno-unused-variable - -Wno-gnu-array-member-paren-init - -Wno-cast-align - -Wno-missing-braces - -Wno-strict-aliasing -# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - ) - -# C-specific warnings -# -set(ARCHCWARNINGS - ${ARCHWARNINGS} - -Wstrict-prototypes - -Wnested-externs - ) - -# C++-specific warnings -# -set(ARCHWARNINGSXX - ${ARCHWARNINGS} - -Wno-missing-field-initializers - ) -exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-libgcc-file-name OUTPUT_VARIABLE LIBGCC) -exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-file-name=libm.a OUTPUT_VARIABLE LIBM) -set(EXTRA_LIBS ${EXTRA_LIBS} ${LIBM}) - -# Flags we pass to the C compiler -# -list2string(CFLAGS - ${ARCHCFLAGS} - ${ARCHCWARNINGS} - ${ARCHOPTIMIZATION} - ${ARCHCPUFLAGS} - ${ARCHINCLUDES} - ${INSTRUMENTATIONDEFINES} - ${ARCHDEFINES} - ${EXTRADEFINES} - ${EXTRACFLAGS} - ${HEXAGON_INCLUDE_DIRS} - ) - -# Flags we pass to the C++ compiler -# -list2string(CXXFLAGS - ${ARCHCXXFLAGS} - ${ARCHWARNINGSXX} - ${ARCHOPTIMIZATION} - ${ARCHCPUFLAGS} - ${ARCHXXINCLUDES} - ${INSTRUMENTATIONDEFINES} - ${ARCHDEFINES} - ${EXTRADEFINES} - ${EXTRACXXFLAGS} - ${HEXAGON_INCLUDE_DIRS} - ) - -# Flags we pass to the assembler -# -list2string(AFLAGS - ${CFLAGS} - -D__ASSEMBLY__ - ${EXTRADEFINES} - ${EXTRAAFLAGS} - ) - -# Set cmake flags -# -list2string(CMAKE_C_FLAGS - ${CMAKE_C_FLAGS} - ${CFLAGS} - ) - -set(QURT_CMAKE_C_FLAGS ${CMAKE_C_FLAGS} CACHE STRING "cflags") - -message(STATUS "CMAKE_C_FLAGS: -${CMAKE_C_FLAGS}-") - -list2string(CMAKE_CXX_FLAGS - ${CMAKE_CXX_FLAGS} - ${CXXFLAGS} - ) - -set(QURT_CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} CACHE STRING "cxxflags") - -message(STATUS "CMAKE_CXX_FLAGS: -${CMAKE_CXX_FLAGS}-") - -# Flags we pass to the linker -# -list2string(CMAKE_EXE_LINKER_FLAGS - -g - -mv5 - -mG0lib - -G0 - -fpic - -shared - -Wl,-Bsymbolic - -Wl,--wrap=malloc - -Wl,--wrap=calloc - -Wl,--wrap=free - -Wl,--wrap=realloc - -Wl,--wrap=memalign - -Wl,--wrap=__stack_chk_fail - -lc - ${EXTRALDFLAGS} - ) - -# where is the target environment -set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) - -set(CMAKE_C_COMPILER_ID, "Clang") -set(CMAKE_CXX_COMPILER_ID, "Clang") - -# search for programs in the build host directories -set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) -# for libraries and headers in the target directories -set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) -set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index c16ed4f2b9..34832a15e6 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -8,6 +8,7 @@ px4_qurt_generate_builtin_commands( # Enable build without HexagonSDK to check link dependencies if ("${QURT_ENABLE_STUBS}" STREQUAL "1") + add_definitions(-DQURT_EXE_BUILD) add_executable(mainapp ${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c ${CMAKE_BINARY_DIR}/apps.h) diff --git a/src/firmware/qurt/mainapp.idl b/src/firmware/qurt/mainapp.idl new file mode 100755 index 0000000000..c86adac37b --- /dev/null +++ b/src/firmware/qurt/mainapp.idl @@ -0,0 +1,41 @@ +/**************************************************************************** + * 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 ATLFlight 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 MAINAPP_IDL +#define MAINAPP_IDL + +#include "AEEStdDef.idl" + +interface mainapp{ + uint32 test(); +}; + +#endif /*MAINAPP_IDL*/ diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 2ae8d8118d..f77bf5c958 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 2ae8d8118db0e95867cd1946d5b8b85d7e4ef9d3 +Subproject commit f77bf5c9583dcbf2e6e790784fb10b6c120aaf3e diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index 13542cdfd3..325c08a6b5 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -61,14 +61,20 @@ static void do_dlopen() #endif } +#ifdef QURT_EXE_BUILD +int dlinit(int a, char **b) +{ + return 0; +} +#endif int main(int argc, char *argv[]) { int ret = 0; - char *builtin[] = {"libgcc.so", "libc.so", "libstdc++.so"}; + char *builtin[] = {"libgcc.so", "libc.so"}; printf("In DSPAL main\n"); - dlinit(3, builtin); + dlinit(2, builtin); do_dlopen(); return ret; From caab01642562ab87571b4e184eaeb92cd1bea41d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 9 Jan 2016 23:26:28 -0800 Subject: [PATCH 003/105] Updated cmake_hexagon Signed-off-by: Mark Charlebois --- cmake/cmake_hexagon | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon index e64ded44b2..829f22eff3 160000 --- a/cmake/cmake_hexagon +++ b/cmake/cmake_hexagon @@ -1 +1 @@ -Subproject commit e64ded44b248fa8571647e11a4aeed7833ecd8a7 +Subproject commit 829f22eff345c934ca8939b2385768ca5e33794c From 9674c611b362c4da055b2396ad4c54fa9896ecaa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jan 2016 20:33:00 +0100 Subject: [PATCH 004/105] Updated DriverFramework, fixes NuttX build --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index f77bf5c958..acb6d24787 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit f77bf5c9583dcbf2e6e790784fb10b6c120aaf3e +Subproject commit acb6d24787a36419849a046d26d7cc9cb2949665 From b22c05a19d5208b00d0a462049ec2c4449cfa3ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jan 2016 20:40:16 +0100 Subject: [PATCH 005/105] Update DriverFramework and make mutex optional --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index acb6d24787..4297f6f43b 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit acb6d24787a36419849a046d26d7cc9cb2949665 +Subproject commit 4297f6f43b230a4c790d2f805958e554858dea29 From fe07079367015b2964e5e8f79d3cae30d1ddfec5 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 09:11:31 +0100 Subject: [PATCH 006/105] added parameter interface to ekf2 --- src/modules/ekf2/ekf2_main.cpp | 66 +++++++++- src/modules/ekf2/ekf2_params.c | 226 +++++++++++++++++++++++++++++++++ 2 files changed, 288 insertions(+), 4 deletions(-) create mode 100644 src/modules/ekf2/ekf2_params.c diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2bd5d25fa0..27c3e99110 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -71,6 +72,7 @@ #include #include #include +#include #include @@ -86,7 +88,7 @@ Ekf2 *instance; } -class Ekf2 +class Ekf2 : public control::SuperBlock { public: /** @@ -133,6 +135,31 @@ private: math::LowPassFilter2p _lp_pitch_rate; math::LowPassFilter2p _lp_yaw_rate; + control::BlockParamFloat *_mag_delay_ms; + control::BlockParamFloat *_baro_delay_ms; + control::BlockParamFloat *_gps_delay_ms; + control::BlockParamFloat *_airspeed_delay_ms; + control::BlockParamFloat *_requiredEph; + control::BlockParamFloat *_requiredEpv; + + control::BlockParamFloat *_gyro_noise; + control::BlockParamFloat *_accel_noise; + + // process noise + control::BlockParamFloat *_gyro_bias_p_noise; + control::BlockParamFloat *_accel_bias_p_noise; + control::BlockParamFloat *_gyro_scale_p_noise; + control::BlockParamFloat *_mag_p_noise; + control::BlockParamFloat *_wind_vel_p_noise; + + control::BlockParamFloat *_gps_vel_noise; + control::BlockParamFloat *_gps_pos_noise; + control::BlockParamFloat *_baro_noise; + + control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion + control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees + control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test + EstimatorBase *_ekf; @@ -143,14 +170,40 @@ private: }; Ekf2::Ekf2(): -_lp_roll_rate(250.0f, 30.0f), -_lp_pitch_rate(250.0f, 30.0f), -_lp_yaw_rate(250.0f, 20.0f) + SuperBlock(NULL, "EKF"), + _lp_roll_rate(250.0f, 30.0f), + _lp_pitch_rate(250.0f, 30.0f), + _lp_yaw_rate(250.0f, 20.0f) { _ekf = new Ekf(); _att_pub = nullptr; _lpos_pub = nullptr; _control_state_pub = nullptr; + + parameters *params = _ekf->getParamHandle(); + _mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, ¶ms->mag_delay_ms); + _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms); + _gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, ¶ms->gps_delay_ms); + _airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, ¶ms->airspeed_delay_ms); + _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->requiredEph); + _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, ¶ms->requiredEpv); + + _gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, ¶ms->gyro_noise); + _accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, ¶ms->accel_noise); + + _gyro_bias_p_noise = new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, ¶ms->gyro_bias_p_noise); + _accel_bias_p_noise = new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, ¶ms->accel_bias_p_noise); + _gyro_scale_p_noise = new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, ¶ms->gyro_scale_p_noise); + _mag_p_noise = new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, ¶ms->mag_p_noise); + _wind_vel_p_noise = new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, ¶ms->wind_vel_p_noise); + + _gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, ¶ms->gps_vel_noise); + _gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, ¶ms->gps_pos_noise); + _baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, ¶ms->baro_noise); + + _mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, ¶ms->mag_heading_noise); + _mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, ¶ms->mag_declination_deg); + _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, ¶ms->heading_innov_gate); } Ekf2::~Ekf2() @@ -182,6 +235,9 @@ void Ekf2::task_main() fds[0].fd = _sensors_sub; fds[0].events = POLLIN; + // initialise parameter cache + updateParams(); + while (!_task_should_exit) { int ret = px4_poll(fds, 1, 1000); @@ -195,6 +251,8 @@ void Ekf2::task_main() continue; } + updateParams(); + bool gps_updated = false; bool airspeed_updated = false; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c new file mode 100644 index 0000000000..ea6643d329 --- /dev/null +++ b/src/modules/ekf2/ekf2_params.c @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 parameters.c + * Parameter definition for ekf2. + * + * @author Roman Bast + * + */ + +#include + +/** +* Magnetometer measurement delay +* +* @group EKF2 +* @min 0 +* @max 300 +* @unit ms +*/ +PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0); + +/** + * Barometer measurement delay + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); + +/** + * GPS measurement delay + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200); + +/** + * Airspeed measurement delay + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + */ +PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200); + +/** + * Required EPH to use GPS. + * + * @group EKF2 + * @min 2 + * @max 100 + * @unit m + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10); + +/** + * Required EPV to use GPS. + * + * @group EKF2 + * @min 2 + * @max 100 + * @unit m + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 20); + +/** + * Gyro noise. + * + * @group EKF2 + * @min 0.0001 + * @max 0.05 + */ +PARAM_DEFINE_FLOAT(EKF2_G_NOISE, 0.001f); + +/** + * Process noise for delta velocity prediction. + * + * @group EKF2 + * @min 0.01 + * @max 1 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.1f); + +/** + * Process noise for delta angle bias prediction. + * + * @group EKF2 + * @min 0 + * @max 0.0001 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_GB_NOISE, 1e-5f); + +/** + * Process noise for delta velocity z bias prediction. + * + * @group EKF2 + * @min 0.000001 + * @max 0.01 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_ACCB_NOISE, 1e-3f); + +/** + * Process noise for delta angle scale prediction. + * + * @group EKF2 + * @min 0.000001 + * @max 0.01 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_GS_NOISE, 1e-4f); + +/** + * Process noise for earth magnetic field and bias prediction. + * + * @group EKF2 + * @min 0.0001 + * @max 0.1 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 1e-2f); + +/** + * Process noise for wind velocity prediction. + * + * @group EKF2 + * @min 0.01 + * @max 1 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 0.05f); + +/** + * Measurement noise for gps velocity. + * + * @group EKF2 + * @min 0.001 + * @max 0.5 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.05f); + +/** + * Measurement noise for gps position. + * + * @group EKF2 + * @min 0.01 + * @max 5 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 1.0f); + +/** + * Measurement noise for barometer. + * + * @group EKF2 + * @min 0.001 + * @max 1 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 0.1f); + +/** + * Measurement noise for mag heading fusion. + * + * @group EKF2 + * @min 0.0001 + * @max 0.1 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f); + +/** + * Magnetic declination + * + * @group EKF2 + * @unit degrees + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); + +/** + * Gate for maginetic heading fusion + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_H_INOV_GATE, 0.5f); From 8a9b27f8f3e290a62dede0f660650652e0261597 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 10:49:18 +0100 Subject: [PATCH 007/105] ecl ekf: added parameter interface --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 2af5856361..a11709956f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 2af5856361adf7ede444cc7f3fd6cf725dfa0213 +Subproject commit a11709956fd0d082d4fde4da0ad8c79117a38990 From 0510d2cb569b374e694114f6f019b23f677545ef Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 10:51:09 +0100 Subject: [PATCH 008/105] fixed code style --- src/modules/ekf2/ekf2_main.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 27c3e99110..22fce7ec6f 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -352,11 +352,13 @@ void Ekf2::task_main() // Position of local NED origin in GPS / WGS84 frame lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set - lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) - lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) + lpos.xy_global = + _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees - lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + lpos.ref_alt = + _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level // The rotation of the tangent plane vs. geographical north lpos.yaw = 0.0f; @@ -366,7 +368,7 @@ void Ekf2::task_main() lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid - // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres + // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres // TODO: Should use sqrt of filter position variances lpos.eph = gps.eph; lpos.epv = gps.epv; @@ -374,6 +376,7 @@ void Ekf2::task_main() // publish vehicle local position data if (_lpos_pub == nullptr) { _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + } else { orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); } @@ -393,6 +396,7 @@ void Ekf2::task_main() // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + } else { orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); } @@ -411,12 +415,14 @@ void Ekf2::task_main() // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } // generate and publish global position data struct vehicle_global_position_s global_pos; + if (_ekf->position_is_valid()) { // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator @@ -449,6 +455,7 @@ void Ekf2::task_main() if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + } else { orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } @@ -525,7 +532,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "print")) { if (ekf2::instance != nullptr) { - + return 0; } From 88b2c6c78da055d4238088119bf9ce338c752431 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 10 Jan 2016 21:13:30 +0100 Subject: [PATCH 009/105] blockparam: added support for external parameter copy --- src/modules/controllib/block/BlockParam.cpp | 22 +++++++++++++++++---- src/modules/controllib/block/BlockParam.hpp | 3 ++- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index b560d7999e..c3ed27a572 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -82,9 +82,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref template BlockParam::BlockParam(Block *block, const char *name, - bool parent_prefix) : + bool parent_prefix, T *extern_address) : BlockParamBase(block, name, parent_prefix), - _val() + _val(), + _extern_address(extern_address) { update(); } @@ -93,12 +94,25 @@ template T BlockParam::get() { return _val; } template -void BlockParam::set(T val) { _val = val; } +void BlockParam::set(T val) +{ + _val = val; + + if (_extern_address != NULL) { + *_extern_address = val; + } +} template void BlockParam::update() { - if (_handle != PARAM_INVALID) { param_get(_handle, &_val); } + if (_handle != PARAM_INVALID) { + param_get(_handle, &_val); + + if (_extern_address != NULL) { + *_extern_address = _val; + } + } } template diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index db035f9f91..693a8ec3b2 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -77,7 +77,7 @@ class BlockParam : public BlockParamBase { public: BlockParam(Block *block, const char *name, - bool parent_prefix = true); + bool parent_prefix = true, T *extern_address = NULL); T get(); void commit(); void set(T val); @@ -85,6 +85,7 @@ public: virtual ~BlockParam(); protected: T _val; + T *_extern_address; }; typedef BlockParam BlockParamFloat; From a355bdeea31851700c4b8d03f6b0258c433fc17b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jan 2016 23:24:33 +0100 Subject: [PATCH 010/105] Fix MAVLink radio status flow control --- src/modules/mavlink/mavlink_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 233c8cc305..7e751c1cf2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -788,6 +788,7 @@ Mavlink::enable_flow_control(bool enabled) { // We can't do this on USB - skip if (_is_usb_uart) { + _flow_control_enabled = false; return OK; } @@ -1459,7 +1460,7 @@ Mavlink::update_rate_mult() /* scale down if we have a TX err rate suggesting link congestion */ if (_rate_txerr > 0.0f && !radio_critical) { hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr); - } else if (radio_found && tstatus.timestamp != _last_hw_rate_timestamp) { + } else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) { if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { /* this indicates link congestion, reduce rate by 20% */ @@ -1473,12 +1474,13 @@ Mavlink::update_rate_mult() /* limit to a max multiplier of 1 */ hardware_mult = fminf(1.0f, hardware_mult); } - } else { + + } else if (!radio_found) { /* no limitation, set hardware to 1 */ hardware_mult = 1.0f; } - _last_hw_rate_timestamp = tstatus.timestamp; + _last_hw_rate_timestamp = tstatus.telem_time; /* pick the minimum from bandwidth mult and hardware mult as limit */ _rate_mult = fminf(bandwidth_mult, hardware_mult); @@ -2201,6 +2203,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + printf("\tflow control:\t%s\n", (_flow_control_enabled) ? "ON" : "OFF"); } else { printf("\tno telem status.\n"); From 4d36cb848f28f2899f7a97ebb25cf796fd39c0d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Jan 2016 00:38:05 +0100 Subject: [PATCH 011/105] Fix excessive DriverFramework log level --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 4297f6f43b..98c4dd676f 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 4297f6f43b230a4c790d2f805958e554858dea29 +Subproject commit 98c4dd676f8ac8c44a377f95864cb29a992a549e From ecc53488dde217412bb5126e97427df2fe2bccc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Jan 2016 00:40:40 +0100 Subject: [PATCH 012/105] Commander: Allow commandline takeoff if already armed --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 03684847e6..dfadb05e5b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -417,7 +417,7 @@ int commander_main(int argc, char *argv[]) /* see if we got a home position */ if (status.condition_home_position_valid) { - if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + if (TRANSITION_DENIED != arm_disarm(true, mavlink_fd_local, "command line")) { vehicle_command_s cmd = {}; cmd.target_system = status.system_id; From e1125ce9d37a2de0b98829e94512e7462dfe5874 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 21:18:07 +0100 Subject: [PATCH 013/105] Add digicam commands --- msg/vehicle_command.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 3a4fb3a596..47f47bf25e 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -31,6 +31,7 @@ uint32 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |S uint32 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION=185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_DIGICAM_CONTROL=203 uint32 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| uint32 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| From 77782bd254e395e0b59691596d434977c53a2802 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 21:18:32 +0100 Subject: [PATCH 014/105] Navigator: Support transition command and digicam command in missions --- src/modules/navigator/mission_block.cpp | 81 ++++++++++++++----- src/modules/navigator/mission_block.h | 7 +- .../navigator/mission_feasibility_checker.cpp | 5 +- 3 files changed, 72 insertions(+), 21 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 897173d0d9..a58bcebfce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -51,6 +51,7 @@ #include #include +#include #include "navigator.h" #include "mission_block.h" @@ -66,7 +67,8 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _actuators{}, - _actuator_pub(nullptr) + _actuator_pub(nullptr), + _cmd_pub(nullptr) { } @@ -77,27 +79,48 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { - if (_mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_2), &actuators); - memset(&actuators, 0, sizeof(actuators)); - actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_2), actuator_pub_fd, &actuators); - return true; - } + /* handle non-navigation or indefinite waypoints */ + switch (_mission_item.nav_cmd) { + case NAV_CMD_DO_SET_SERVO: { + memset(&actuators, 0, sizeof(actuators)); + actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; + actuators.timestamp = hrt_absolute_time(); + if (_actuator_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + } + return true; + } - if (_mission_item.nav_cmd == NAV_CMD_IDLE) { - return false; - } + case NAV_CMD_LAND: + return _navigator->get_vstatus()->condition_landed; - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _navigator->get_vstatus()->condition_landed; - } + /* TODO: count turns */ + /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + case NAV_CMD_IDLE: /* fall through */ + case NAV_CMD_LOITER_UNLIMITED: + return false; - /* TODO: count turns */ - if (/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - return false; + case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */ + case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: + { + /* forward the command to other processes */ + warnx("got instantaneous command, forwarding.\n"); + struct vehicle_command_s cmd = {}; + cmd.command = _mission_item.nav_cmd; + mission_item_to_vehicle_command(&_mission_item, &cmd); + if (_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + } else { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + } + return true; + } + + default: + /* do nothing, this is a 3D waypoint */ + break; } hrt_abstime now = hrt_absolute_time(); @@ -210,6 +233,26 @@ MissionBlock::reset_mission_item_reached() _time_first_inside_orbit = 0; } +void +MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd) +{ + // Assignment based on MAVLink spec for param enumeration: + // http://mavlink.org/messages/common#MAV_CMD_NAV_WAYPOINT + cmd->param1 = item->time_inside; + cmd->param2 = item->acceptance_radius; + cmd->param3 = item->loiter_radius; + cmd->param4 = item->yaw; + cmd->param5 = item->lat; + cmd->param6 = item->lon; + cmd->param7 = item->altitude; + + cmd->target_system = _navigator->get_vstatus()->system_id; + cmd->target_component = _navigator->get_vstatus()->component_id; + cmd->source_system = _navigator->get_vstatus()->system_id; + cmd->source_component = _navigator->get_vstatus()->component_id; + cmd->confirmation = false; +} + void MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 36a1ecaba1..79845c781b 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -111,6 +111,11 @@ protected: */ void set_idle_item(struct mission_item_s *item); + /** + * Convert a mission item to a command + */ + void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -118,7 +123,7 @@ protected: actuator_controls_s _actuators; orb_advert_t _actuator_pub; - + orb_advert_t _cmd_pub; }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ece75c89f7..fd9a206c86 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -50,6 +50,7 @@ #include #include #include +#include MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), @@ -199,7 +200,9 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_TAKEOFF && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && missionitem.nav_cmd != NAV_CMD_DO_JUMP && - missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO) { + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; From b0333e3e9598a423ee71a57c2b8f239fa4d48170 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 6 Jan 2016 15:38:44 +0100 Subject: [PATCH 015/105] allow transition commands in auto mode --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 65ab5f6233..b6b2cdade0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -423,7 +423,7 @@ VtolAttitudeControl::is_fixed_wing_requested() { bool to_fw = _manual_control_sp.aux1 > 0.0f; - if (_v_control_mode.flag_control_offboard_enabled) { + if (_v_control_mode.flag_control_offboard_enabled || _v_control_mode.flag_control_auto_enabled) { to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; } From 7e7b21cbdc678e3ac006162275edc1a79021123f Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 6 Jan 2016 15:39:04 +0100 Subject: [PATCH 016/105] use raw parameter floats if mission item is a mission command --- src/modules/mavlink/mavlink_mission.cpp | 7 +++++++ src/modules/navigator/mission_block.cpp | 18 +++++++++--------- src/modules/navigator/navigation.h | 1 + 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 746689dd52..073c5afcfc 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -780,6 +780,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_FRAME_MISSION: // This is a mission item with no coordinate + mission_item->params[0] = mavlink_mission_item->param1; + mission_item->params[1] = mavlink_mission_item->param2; + mission_item->params[2] = mavlink_mission_item->param3; + mission_item->params[3] = mavlink_mission_item->param4; + mission_item->params[4] = mavlink_mission_item->x; + mission_item->params[5] = mavlink_mission_item->y; + mission_item->params[6] = mavlink_mission_item->z; break; default: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a58bcebfce..64512d3d3d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -236,15 +236,15 @@ MissionBlock::reset_mission_item_reached() void MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd) { - // Assignment based on MAVLink spec for param enumeration: - // http://mavlink.org/messages/common#MAV_CMD_NAV_WAYPOINT - cmd->param1 = item->time_inside; - cmd->param2 = item->acceptance_radius; - cmd->param3 = item->loiter_radius; - cmd->param4 = item->yaw; - cmd->param5 = item->lat; - cmd->param6 = item->lon; - cmd->param7 = item->altitude; + // we're expecting a mission command item here so assign the "raw" inputs to the command + // (MAV_FRAME_MISSION mission item) + cmd->param1 = item->params[0]; + cmd->param2 = item->params[1]; + cmd->param3 = item->params[2]; + cmd->param4 = item->params[3]; + cmd->param5 = item->params[4]; + cmd->param6 = item->params[5]; + cmd->param7 = item->params[6]; cmd->target_system = _navigator->get_vstatus()->system_id; cmd->target_component = _navigator->get_vstatus()->component_id; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 89f77b4752..b768de5ac5 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -103,6 +103,7 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ int actuator_value; /**< new value for selected actuator in ms 900...2000 */ + float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ }; #pragma pack(pop) #include From 9d7b3bbff172f899501e75f7327abe9000cc1c72 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:17:09 +0100 Subject: [PATCH 017/105] update ublox Protocol Specification links update ublox Protocol Specification links --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f6c3fd7882..901c761b47 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -44,8 +44,8 @@ * @author Hannes Delago * (rework, add ubx7+ compatibility) * - * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf - * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + * @see https://www2.u-blox.com/images/downloads/Product_Docs/u-blox6-GPS-GLONASS-QZSS-V14_ReceiverDescriptionProtocolSpec_Public_(GPS.G6-SW-12013).pdf + * @see https://www.u-blox.com/sites/default/files/products/documents/u-bloxM8_ReceiverDescrProtSpec_%28UBX-13003221%29_Public.pdf */ #include From 902b774091046cba4bd8f289320a9035688fb720 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:36:55 +0100 Subject: [PATCH 018/105] Update mavlink_px4.py spelling mistakes --- Tools/mavlink_px4.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/mavlink_px4.py b/Tools/mavlink_px4.py index bbad57b7d4..b9f17cb5e7 100644 --- a/Tools/mavlink_px4.py +++ b/Tools/mavlink_px4.py @@ -752,7 +752,7 @@ class MAVLink_gps_raw_int_message(MAVLink_message): ''' The global position, as returned by the Global Positioning System (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. See + estimate of the system, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame). ''' @@ -2812,7 +2812,7 @@ class MAVLink(object): ''' The global position, as returned by the Global Positioning System (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. + estimate of the system, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame). @@ -2837,7 +2837,7 @@ class MAVLink(object): ''' The global position, as returned by the Global Positioning System (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. + estimate of the system, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame). From fb702b7c487503138cd04d036bf7fddde79f5a2e Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:39:00 +0100 Subject: [PATCH 019/105] Update input_rc.msg spelling mistakes --- msg/input_rc.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 8e15a9a186..3e226966e6 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -14,7 +14,7 @@ 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. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usUally 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 From caf0121f95e27561da51b3b05141a937de9ede9c Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:39:48 +0100 Subject: [PATCH 020/105] Update manual_control_setpoint.msg spelling mistakes --- msg/manual_control_setpoint.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index dd1f7d0b5e..2ff6fca1f5 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -26,7 +26,7 @@ float32 z # throttle stick position 0..1 # in general the value corresponds to the demanded throttle by the user, # if the input is used for setting the setpoint of a vertical position # controller any value > 0.5 means up and any value < 0.5 means down -float32 r # yaw stick/twist positon, -1..1 +float32 r # yaw stick/twist position, -1..1 # in general corresponds to the righthand rotation around the vertical # (downwards) axis of the vehicle float32 flaps # flap position From 9197b85cfeb2101f10f0af0bc43c1f4dbed78e3c Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:40:18 +0100 Subject: [PATCH 021/105] Update rc_parameter_map.msg spelling mistakes --- msg/rc_parameter_map.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg index b2dd1843ab..4a94498108 100644 --- a/msg/rc_parameter_map.msg +++ b/msg/rc_parameter_map.msg @@ -6,6 +6,6 @@ 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] value0 # initial value around which the parameter value is changed float32[3] value_min # minimal parameter value float32[3] value_max # minimal parameter value From 9f009d061556612d833f6d1fa6a23275c83ffdba Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:41:49 +0100 Subject: [PATCH 022/105] Update UserGuide.md spelling mistakes --- Documentation/px4_hil/UserGuide.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/px4_hil/UserGuide.md b/Documentation/px4_hil/UserGuide.md index dd76d3e3a9..28483498da 100644 --- a/Documentation/px4_hil/UserGuide.md +++ b/Documentation/px4_hil/UserGuide.md @@ -48,7 +48,7 @@ Download QGC from http://qgroundcontrol.org/downloads and install using the wind ## PX4 -### Platfrom Requirements +### Platform Requirements Linux or Eagle with a working IP interface (?? does this need further instructions?) ### Build Host Requirements From b274a3f6d138dbc8cd88e85870a1fdb77abbd747 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:43:19 +0100 Subject: [PATCH 023/105] Update test_eigen.cpp spelling mistakes --- src/systemcmds/tests/test_eigen.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index ba3922784b..2ede1ad66c 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -226,7 +226,7 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector3f length squared", v1.squaredNorm()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" - // Need pragma here intead of moving variable out of TEST_OP and just reference because + // Need pragma here instead of moving variable out of TEST_OP and just reference because // TEST_OP measures performance of vector operations. TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]); From 8701e0ba78a2caf485d02dfd9fb1a040f08d8f02 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:44:12 +0100 Subject: [PATCH 024/105] Update test_mathlib.cpp spelling mistakes --- src/systemcmds/tests/test_mathlib.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index dcd03e1be9..0883f42e8a 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -100,7 +100,7 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> length squared", v1.length_squared()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" - // Need pragma here intead of moving variable out of TEST_OP and just reference because + // Need pragma here instead of moving variable out of TEST_OP and just reference because // TEST_OP measures performance of vector operations. TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); From 5c1b84f16a8f90a91fb19791ee6ec6392b85ebe3 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:45:15 +0100 Subject: [PATCH 025/105] Update ms5611_i2c.cpp spelling mistakes --- src/drivers/ms5611/ms5611_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 3a86a7f064..21f986b4c9 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -265,7 +265,7 @@ MS5611_I2C::_read_prom() /* check if all bytes are zero */ if (i == 0) { - /* initalize to first byte read */ + /* initialize to first byte read */ last_val = prom_buf[0]; } From 86ca7d8cfcdd608f3ffbd515f644a1ad073ee419 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:46:31 +0100 Subject: [PATCH 026/105] Update drv_hrt.c spelling mistakes --- src/drivers/stm32/drv_hrt.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 475c9c9c75..3bea3fda05 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -599,7 +599,7 @@ error: #endif /* HRT_PPM_CHANNEL */ /** - * Handle the compare interupt by calling the callout dispatcher + * Handle the compare interrupt by calling the callout dispatcher * and then re-scheduling the next deadline. */ static int @@ -750,7 +750,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now) } /** - * Initalise the high-resolution timing module. + * Initialise the high-resolution timing module. */ void hrt_init(void) From 1d1ad3e9ed790cd73cd52bf54e40aee08988a898 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:47:29 +0100 Subject: [PATCH 027/105] Update messages.h spelling mistakes --- src/drivers/hott/messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index 504c9f19ec..ab1bef6c98 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -211,7 +211,7 @@ struct gps_module_msg { uint8_t resolution_L; /**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ uint8_t resolution_H; /**< 117 = High Byte m/s resolution 0.01m */ uint8_t unknown1; /**< 120 = 0m/3s */ - uint8_t gps_num_sat; /**< GPS.Satellites (number of satelites) (1 byte) */ + uint8_t gps_num_sat; /**< GPS.Satellites (number of satellites) (1 byte) */ uint8_t gps_fix_char; /**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ uint8_t home_direction; /**< HomeDirection (direction from starting point to Model position) (1 byte) */ uint8_t angle_x_direction; /**< angle x-direction (1 byte) */ From 54677cd223f00ea99c0a5f05dfabe798f0dd8fc2 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:48:10 +0100 Subject: [PATCH 028/105] Update messages.cpp spelling mistakes --- src/drivers/hott/messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index db225a2b98..1150fd0722 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -291,7 +291,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - /* Get any (and probably only ever one) _home_sub postion report */ + /* Get any (and probably only ever one) _home_sub position report */ bool updated; orb_check(_home_sub, &updated); From abf03c1c9da69c83f19af31132183976763c9d4c Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:49:00 +0100 Subject: [PATCH 029/105] Update hott_sensors.cpp spelling mistakes --- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index f5c947d4f6..390a64f2ab 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -182,7 +182,7 @@ hott_sensors_thread_main(int argc, char *argv[]) recv_data(uart, &buffer[0], &size, &id); - // Determine which moduel sent it and process accordingly. + // Determine which module sent it and process accordingly. if (id == GAM_SENSOR_ID) { publish_gam_message(buffer); From 4ab4f1edc764728ccf4c3fd3fa7013b811f2c8f9 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:50:33 +0100 Subject: [PATCH 030/105] Update pwm_input.cpp spelling mistakes --- src/drivers/pwm_input/pwm_input.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 1a609a91f0..6aae11cc20 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -536,7 +536,7 @@ void PWMIN::print_info(void) /* - * Handle the interupt, gathering pulse data + * Handle the interrupt, gathering pulse data */ static int pwmin_tim_isr(int irq, void *context) { From c42b0e72013b9817ff54e098e415282b9d0727d9 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:51:34 +0100 Subject: [PATCH 031/105] Update camera_trigger.cpp spelling mistakes --- src/drivers/camera_trigger/camera_trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index d07ec24684..f067fa3e5f 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -34,7 +34,7 @@ /** * @file camera_trigger.cpp * - * External camera-IMU synchronisation and triggering via FMU auxillary pins. + * External camera-IMU synchronisation and triggering via FMU auxiliary pins. * * @author Mohammed Kabir */ From b97b3c68cb6618561cc7c77ef7bd9efe2cb9ccd6 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:52:16 +0100 Subject: [PATCH 032/105] Update mtk.h spelling mistakes --- src/drivers/gps/mtk.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index a2d5e27bbf..a913d3d89f 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -72,7 +72,7 @@ typedef struct { uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 uint32_t ground_speed; ///< velocity in m/s int32_t heading; ///< heading in degrees * 10^2 - uint8_t satellites; ///< number of sattelites used + uint8_t satellites; ///< number of satellites used uint8_t fix_type; ///< fix type: XXX correct for that uint32_t date; uint32_t utc_time; From c341f94e5daeb7845d793ce181ba5956029e9494 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:53:35 +0100 Subject: [PATCH 033/105] Update gimbal_params.c spelling mistakes --- src/drivers/gimbal/gimbal_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c index 955bfa8b23..def83c0ef8 100644 --- a/src/drivers/gimbal/gimbal_params.c +++ b/src/drivers/gimbal/gimbal_params.c @@ -55,11 +55,11 @@ PARAM_DEFINE_INT32(GMB_USE_MNT, 0); /** - * Auxilary switch to set mount operation mode. + * Auxiliary switch to set mount operation mode. * * Set to 0 to disable manual mode control. * - * If set to an auxilary switch: + * If set to an auxiliary switch: * Switch off means the gimbal is put into safe/locked position. * Switch on means the gimbal can move freely, and landing gear * will be retracted if applicable. From 07973bf87a12b854eb66bcb73c17e7356cc9890c Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:56:10 +0100 Subject: [PATCH 034/105] Update px4_nodehandle.h spelling mistakes --- src/platforms/px4_nodehandle.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index dc806aa905..31afbfd031 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -132,7 +132,7 @@ public: void spinOnce() { ros::spinOnce(); } /** - * Keeps calling callbacks for incomming messages, returns when module is terminated + * Keeps calling callbacks for incoming messages, returns when module is terminated */ void spin() { ros::spin(); } @@ -266,7 +266,7 @@ public: } /** - * Keeps calling callbacks for incomming messages, returns when module is terminated + * Keeps calling callbacks for incoming messages, returns when module is terminated */ void spin() { @@ -292,7 +292,7 @@ protected: static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ List _pubs; /**< Publications of node */ - SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + SubscriberNode *_sub_min_interval; /**< Points to the sub with the smallest interval of all Subscriptions in _subs*/ AppState &_appState; From 70e7f1bec6275f4fa2dbe5d4cb3533e5737e8a88 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:57:26 +0100 Subject: [PATCH 035/105] Update px4_spi.h spelling mistakes --- src/platforms/px4_spi.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 78b747b775..02f4e133e4 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -17,7 +17,7 @@ enum spi_dev_e { SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ }; -/* Certain SPI devices may required differnt clocking modes */ +/* Certain SPI devices may required different clocking modes */ enum spi_mode_e { SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ From d5c6fde5bcc1eb1c4e2a2bd9c8392c7bdf12bf9b Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:58:05 +0100 Subject: [PATCH 036/105] Update px4_posix.h spelling mistakes --- src/platforms/px4_posix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 9f48524b45..61de2a3fe6 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -129,7 +129,7 @@ typedef struct { pollevent_t events; /* The input event flags */ pollevent_t revents; /* The output event flags */ - /* Required for PX4 compatability */ + /* Required for PX4 compatibility */ px4_sem_t *sem; /* Pointer to semaphore used to post output event */ void *priv; /* For use by drivers */ } px4_pollfd_struct_t; From 41abe9221eca1a683624fd68d6d00dc46b659190 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 11:59:39 +0100 Subject: [PATCH 037/105] Update main.cpp spelling mistakes --- src/platforms/posix/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 4d6663006d..749feaa6a3 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -35,7 +35,7 @@ * Basic shell to execute builtin "apps" * * @author Mark Charlebois - * @auther Roman Bapst + * @author Roman Bapst */ #include @@ -238,7 +238,7 @@ int main(int argc, char **argv) const char *folderpath = "/rootfs/"; if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) { - PX4_ERR("Failed aquiring working dir, abort."); + PX4_ERR("Failed acquiring working dir, abort."); exit(1); } From 6e1bf506ad822d001e1aa707c79f5ee9ca73ca1a Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 12:00:26 +0100 Subject: [PATCH 038/105] Update dq_rem.c spelling mistakes --- src/platforms/posix/work_queue/dq_rem.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/work_queue/dq_rem.c b/src/platforms/posix/work_queue/dq_rem.c index e6c9bbed37..4249daec50 100644 --- a/src/platforms/posix/work_queue/dq_rem.c +++ b/src/platforms/posix/work_queue/dq_rem.c @@ -51,7 +51,7 @@ /************************************************************ * Name: dq_rem * - * Descripton: + * Description: * dq_rem removes 'node' from 'queue' * ************************************************************/ From 1742cce6f6e270eb48a070fde1b940091aed7023 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 12:01:07 +0100 Subject: [PATCH 039/105] Update commander.h spelling mistakes --- src/platforms/ros/nodes/commander/commander.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 67bee544ab..ea8e2de7fd 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -72,7 +72,7 @@ protected: px4::vehicle_control_mode &msg_vehicle_control_mode); /** - * Sets offboard controll flags in msg_vehicle_control_mode + * Sets offboard control flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, px4::vehicle_control_mode &msg_vehicle_control_mode); From 3dc4411592073147367c76c1b51094312a5f9050 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 12:02:31 +0100 Subject: [PATCH 040/105] Update sensors.cpp spelling mistakes --- src/modules/sensors/sensors.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b20b33011e..b5aa7f56fa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -185,7 +185,7 @@ private: switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** - * Update paramters from RC channels if the functionality is activated and the + * Update parameters from RC channels if the functionality is activated and the * input has changed since the last update * * @param @@ -346,7 +346,7 @@ private: param_t rc_map_aux5; 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 + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound to a RC channel, equivalent float values in the _parameters struct are not existing because these parameters are never read. */ @@ -1568,7 +1568,7 @@ Sensors::rc_parameter_map_poll(bool forced) if (map_updated) { orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); - /* update paramter handles to which the RC channels are mapped */ + /* update parameter handles to which the RC channels are mapped */ 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) From 47786c858572761366925fbbe0f57f0b9654cb3f Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 12:04:03 +0100 Subject: [PATCH 041/105] Update mavlink_main.cpp spelling mistakes --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e751c1cf2..4eda4edb6c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -844,7 +844,7 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; - // if we are using network sockets, return max lenght of one packet + // if we are using network sockets, return max length of one packet if (get_protocol() == UDP || get_protocol() == TCP ) { return 1500; } else { @@ -1046,7 +1046,7 @@ Mavlink::init_udp() return; } - /* set default target address, but not for onboard mode (will be set on first recieved packet) */ + /* set default target address, but not for onboard mode (will be set on first received packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); if (_mode != MAVLINK_MODE_ONBOARD) { set_client_source_initialized(); From ed081ef60be2e06f362c114e81613ab2311297c5 Mon Sep 17 00:00:00 2001 From: Stefan Date: Sun, 10 Jan 2016 12:13:02 +0100 Subject: [PATCH 042/105] correct link to developer guide correct link to developer guide --- CONTRIBUTING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index fef02e6228..716f2f432e 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname ### Edit and build the code -The [developer guide](http://px4.io/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://px4.io/dev/code_style) when editing files. +The [developer guide](http://dev.px4.io/) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://px4.io/dev/code_style) when editing files. ### Commit your changes From 78f9bb79f129b21588290827592aac05c8f69361 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Sun, 10 Jan 2016 12:41:04 +0100 Subject: [PATCH 043/105] more spelling mistakes --- Documentation/Doxyfile | 2 +- src/lib/launchdetection/LaunchMethod.h | 2 +- src/lib/launchdetection/launchdetection_params.c | 4 ++-- src/lib/rc/sbus.c | 2 +- src/modules/attitude_estimator_ekf/AttitudeEKF.m | 2 +- src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c | 2 +- .../commander/commander_tests/state_machine_helper_test.cpp | 4 ++-- .../local_position_estimator/BlockLocalPositionEstimator.hpp | 2 +- src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 2 +- src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp | 2 +- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/up_cxxinitialize.c | 2 +- src/modules/systemlib/uthash/doc/userguide.txt | 2 +- 13 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index b674fbc485..fad7cab292 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -281,7 +281,7 @@ TYPEDEF_HIDES_STRUCT = NO # causing a significant performance penality. # If the system has enough physical memory increasing the cache will improve the # performance by keeping more symbols in memory. Note that the value works on -# a logarithmic scale so increasing the size by one will rougly double the +# a logarithmic scale so increasing the size by one will roughly double the # memory usage. The cache size is given by this formula: # 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, # corresponding to a cache size of 2^16 = 65536 symbols diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 8b5220cb3c..66ee3f2c6e 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -51,7 +51,7 @@ enum LaunchDetectionResult { up and still be set to 'throttlePreTakeoff'. For instance this is used to have a delay for the motor when launching a fixed wing aircraft from a bungee */ - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control attitude and also throttle up the motors. */ }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index e5070eeaaf..22650bc678 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); /** - * Catapult accelerometer theshold. + * Catapult accelerometer threshold. * * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. * @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); /** - * Catapult time theshold. + * Catapult time threshold. * * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. * diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index fe72bfb959..bfec926bce 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -615,7 +615,7 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, - * e.g. by prematurely issueing return-to-launch!!! */ + * e.g. by prematurely issuing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m index fea1a773e3..cf559919ae 100644 --- a/src/modules/attitude_estimator_ekf/AttitudeEKF.m +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -2,7 +2,7 @@ function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) -%LQG Postion Estimator and Controller +%LQG Position Estimator and Controller % Observer: % x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) % x[n+1|n] = Ax[n|n] + Bu[n] diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c index 68db382cfe..25bdde5cf1 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -499,7 +499,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char float b_z[6]; - /* LQG Postion Estimator and Controller */ + /* LQG Position Estimator and Controller */ /* Observer: */ /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ /* x[n+1|n] = Ax[n|n] + Bu[n] */ diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 967304cb45..2611136347 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -75,11 +75,11 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) } ArmingTransitionVolatileState_t; // This structure represents a test case for arming_state_transition. It contains the machine - // state prior to transtion, the requested state to transition to and finally the expected + // state prior to transition, the requested state to transition to and finally the expected // machine state after transition. typedef struct { const char* assertMsg; // Text to show when test case fails - ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion + ArmingTransitionVolatileState_t current_state; // Machine state prior to transition hil_state_t hil_state; // Current vehicle_status_s.hil_state bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized bool safety_switch_available; // Current safety_s.safety_switch_available diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 1c6c43fd20..18b373f6b4 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -145,7 +145,7 @@ private: // predict the next state void predict(); - // correct the state prediction wtih a measurement + // correct the state prediction with a measurement void correctBaro(); void correctGps(); void correctLidar(); diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index cd8648f08a..08266a3f07 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -35,7 +35,7 @@ #include #include -// static intialization. +// static initialization. uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; static unsigned long _dropped_pkts; diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 71d052b4c6..4fbb3727f5 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -44,7 +44,7 @@ uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics); -// static intialization. +// static initialization. static std::string _log_file_name = "./hex_dump.txt"; static unsigned long _snd_msg_min = 0xFFFFFF; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 5ed152127a..5063633cc0 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -481,7 +481,7 @@ public: * compared to thrust. * @param yaw_wcale Scaling factor applied to yaw inputs compared * to thrust. - * @param idle_speed Minumum rotor control output value; usually + * @param idle_speed Minimum rotor control output value; usually * tuned to ensure that rotors never stall at the * low end of their control range. */ diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index 6460b03c9d..b496a87bd1 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -118,7 +118,7 @@ extern uint32_t _etext; * This function should then be called in the application-specific * user_start logic in order to perform the C++ initialization. NOTE * that no component of the core NuttX RTOS logic is involved; This - * function defintion only provides the 'contract' between application + * function definition only provides the 'contract' between application * specific C++ code and platform-specific toolchain support * ***************************************************************************/ diff --git a/src/modules/systemlib/uthash/doc/userguide.txt b/src/modules/systemlib/uthash/doc/userguide.txt index 3e65a52fc9..2d746593f2 100644 --- a/src/modules/systemlib/uthash/doc/userguide.txt +++ b/src/modules/systemlib/uthash/doc/userguide.txt @@ -1058,7 +1058,7 @@ always used with the `users_by_name` hash table). Several sort orders ~~~~~~~~~~~~~~~~~~~ -It comes as no suprise that two hash tables can have different sort orders, but +It comes as no surprise that two hash tables can have different sort orders, but this fact can also be used advantageously to sort the 'same items' in several ways. This is based on the ability to store a structure in several hash tables. From f99d0525826fca2997673efef808fc4ef999d9c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Jan 2016 09:27:48 +0100 Subject: [PATCH 044/105] Fix submodule force. Fixes #3490. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 2a5ab238f2..a87daca8f3 100644 --- a/Makefile +++ b/Makefile @@ -109,7 +109,7 @@ endif # describe how to build a cmake config define cmake-build +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef From 79e7059eaf42037cdfa495cdb25ce9914e7af9dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jan 2016 23:30:16 +0100 Subject: [PATCH 045/105] Fix yaw when in manual mode during arming --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 bfc368367b..71db251293 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1760,7 +1760,8 @@ MulticopterPositionControl::task_main() } /* do not move yaw while sitting on the ground */ - else if (!_vehicle_status.condition_landed) { + else if (!_vehicle_status.condition_landed && + !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.05f)) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; From 21b99b408cec117c994b7af017c41614aa92a1dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Jan 2016 11:57:55 +0100 Subject: [PATCH 046/105] Yaw fix: increase threshold --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 71db251293..a604b4c263 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1761,7 +1761,7 @@ MulticopterPositionControl::task_main() /* do not move yaw while sitting on the ground */ else if (!_vehicle_status.condition_landed && - !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.05f)) { + !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; From c40c91bedc1324f916ef50b2a99b5f95dd6519c0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 11 Jan 2016 22:12:32 -0600 Subject: [PATCH 047/105] Updated matrix for quaternion merge. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index b9d6cac199..cc800454d2 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit b9d6cac199bd4476c6268f7eba76c01bf29f0295 +Subproject commit cc800454d2b48f7101608e99dac267d174d58697 From 365ef883e36467c257b40f64675ffaf1608581b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 10:14:15 +0100 Subject: [PATCH 048/105] Check submodules during each build --- Makefile | 1 + Tools/check_submodules.sh | 68 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+) create mode 100755 Tools/check_submodules.sh diff --git a/Makefile b/Makefile index a87daca8f3..56b4f663a6 100644 --- a/Makefile +++ b/Makefile @@ -109,6 +109,7 @@ endif # describe how to build a cmake config define cmake-build +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi ++Tools/check_submodules.sh +@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh new file mode 100755 index 0000000000..51844c4723 --- /dev/null +++ b/Tools/check_submodules.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash + +[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { + # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules + echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + +GITSTATUS=$(git status) + +function check_git_submodule { + +if [ -d $1 ]; + then + SUBMODULE_STATUS=$(git submodule summary "$1") + STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1" | grep "<") + if [ -z "$STATUSRETVAL" ]; then + echo "Checked $1 submodule, correct version found" + else + echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:" + echo "" + echo "" + echo -e "New commits required:" + echo -e "$SUBMODULE_STATUS\033[0m" + echo "" + echo "" + echo " $1 submodule not at correct version. Did you intentionally change the submodule?" + echo " If yes, hit 'y' and to continue the build. If not, hit to abort." + echo -e " Hit 'u' and to update ALL submodules and resolve this (performs \033[94mgit submodule update --init --recursive\033[0m)." + echo -e " Use \033[94mgit add $1 && git commit -m 'Updated $1'\033[0m to choose this submodule version (careful!)" + echo "" + read user_cmd + if [ "$user_cmd" == "y" ] + then + echo "Continuing build with manually overridden submodule.." + else + if [ "$user_cmd" == "u" ] + then + git submodule update --init --recursive + echo "Submodule fixed, continuing build.." + else + echo "Build aborted." + exit 1 + fi + fi + fi +else + git submodule update --init --recursive; + git submodule update; +fi + +} + +check_git_submodule NuttX +check_git_submodule Tools/gencpp +check_git_submodule Tools/genmsg +check_git_submodule Tools/jMAVSim +check_git_submodule Tools/sitl_gazebo +check_git_submodule cmake/cmake_hexagon +check_git_submodule mavlink/include/mavlink/v1.0 +check_git_submodule src/lib/DriverFramework +check_git_submodule src/lib/dspal +check_git_submodule src/lib/ecl +check_git_submodule src/lib/matrix +check_git_submodule src/modules/uavcan/libuavcan +check_git_submodule unittests/googletest + +exit 0 \ No newline at end of file From e9dd2aec48041ddb5039404267cf6e92d4894119 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 10:18:05 +0100 Subject: [PATCH 049/105] Further git submodule improvements --- .travis.yml | 1 + Makefile | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 92ac917fa0..ac98a24261 100644 --- a/.travis.yml +++ b/.travis.yml @@ -93,6 +93,7 @@ env: - PX4_AWS_BUCKET=px4-travis script: + - git submodule update --init --recursive - make check_format - arm-none-eabi-gcc --version - echo 'Building POSIX Firmware..' && make posix_sitl_default diff --git a/Makefile b/Makefile index 56b4f663a6..667897ed4b 100644 --- a/Makefile +++ b/Makefile @@ -109,8 +109,9 @@ endif # describe how to build a cmake config define cmake-build +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi ++git submodule init +Tools/check_submodules.sh -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule sync && git submodule init && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef From 7d7333cdeafe16e122cbffa57f74cce3f600129d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 10:33:24 +0100 Subject: [PATCH 050/105] Better output on submodule check fail --- Tools/check_submodules.sh | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 51844c4723..430c2270f1 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -17,17 +17,23 @@ if [ -d $1 ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked $1 submodule, correct version found" else - echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:" - echo "" + echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:\033[0m" echo "" echo -e "New commits required:" - echo -e "$SUBMODULE_STATUS\033[0m" + echo -e "$SUBMODULE_STATUS" echo "" echo "" - echo " $1 submodule not at correct version. Did you intentionally change the submodule?" - echo " If yes, hit 'y' and to continue the build. If not, hit to abort." - echo -e " Hit 'u' and to update ALL submodules and resolve this (performs \033[94mgit submodule update --init --recursive\033[0m)." - echo -e " Use \033[94mgit add $1 && git commit -m 'Updated $1'\033[0m to choose this submodule version (careful!)" + echo -e " *******************************************************************************" + echo -e " * \033[31mIF YOU DID NOT CHANGE THIS FILE (OR YOU DON'T KNOW WHAT A SUBMODULE IS):\033[0m *" + echo -e " * \033[31mHit 'u' and to update ALL submodules and resolve this.\033[0m *" + echo -e " * (performs \033[94mgit submodule update --init --recursive\033[0m) *" + echo -e " *******************************************************************************" + echo "" + echo "" + echo -e " Only for EXPERTS:" + echo -e " $1 submodule is not in the recommended version." + echo -e " Hit 'y' and to continue the build with this version. Hit to resolve manually." + echo -e " Use \033[94mgit add $1 && git commit -m 'Updated $1'\033[0m to choose this version (careful!)" echo "" read user_cmd if [ "$user_cmd" == "y" ] From e21062c6dbc40aef633a6227df6a87a40b0cfc36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 11:04:53 +0100 Subject: [PATCH 051/105] Updated Matrix lib --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index cc800454d2..67c8c8b331 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit cc800454d2b48f7101608e99dac267d174d58697 +Subproject commit 67c8c8b331d0fb311fbc351fda88d940865d6f98 From e1ce6819601204a6ba66c6e3b18542a27c672447 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 11:52:00 +0100 Subject: [PATCH 052/105] Update Matrix lib for CI --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 67c8c8b331..187830164d 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 67c8c8b331d0fb311fbc351fda88d940865d6f98 +Subproject commit 187830164d6b43fca7f7de39040367ebb387d68f From 2c786e21f410ee66e931b77b793002ca2b7f2faf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Jan 2016 12:16:24 +0100 Subject: [PATCH 053/105] Update Matrix lib to fix isfinite call --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 187830164d..1d9d8e6f9b 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 187830164d6b43fca7f7de39040367ebb387d68f +Subproject commit 1d9d8e6f9b0c2ed930ea1c8996f5fb398b3cf322 From 181cbd383cf51ac6e2e7f55151461c0fb9c97001 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 12 Jan 2016 12:56:57 +0100 Subject: [PATCH 054/105] tailsitter gazebo: lower max climb/descend velocity --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index 1f4fa0ad60..e2dbbbefa4 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -27,6 +27,7 @@ param set MPC_XY_P 0.15 param set MPC_XY_VEL_P 0.05 param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 +param set MPC_Z_VEL_MAX 1.0 param set SENS_BOARD_ROT 8 param set COM_RC_IN_MODE 1 rgbledsim start From a2758eadb6de453c7a3c53c6dc412540cb5d66d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 11:50:27 +0100 Subject: [PATCH 055/105] Automate initial submodule update --- Tools/check_submodules.sh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 430c2270f1..cac39664ee 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -6,6 +6,13 @@ exit 0 } +if [ -f src/modules/uavcan/libuavcan/CMakeLists.txt ] +then + echo "Git submodule config valid." +else + git submodule update --init --recursive +fi + GITSTATUS=$(git status) function check_git_submodule { From 1772cbe5e090c5f0ec6cbee5c634efac53f07fef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 21:14:35 +0100 Subject: [PATCH 056/105] Support reboot_required tag --- Tools/px4params/srcparser.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index b67ad890a9..6abdf7e862 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -107,7 +107,7 @@ class SourceParser(object): re_remove_dots = re.compile(r'\.+$') re_remove_carriage_return = re.compile('\n+') - valid_tags = set(["group", "board", "min", "max", "unit", "decimal"]) + valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "reboot_required"]) # Order of parameter groups priority = { From 9f4e6e32083bc5ce316fedaac5b394d154311216 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 21:14:50 +0100 Subject: [PATCH 057/105] Set reboot required tag for cam trigger --- src/drivers/camera_trigger/camera_trigger_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 507d1972a7..cbdcaf7020 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -79,6 +79,8 @@ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); * * 0 disables the trigger, 1 sets it to enabled on command, 2 always on * + * @reboot_required true + * * @min 0 * @max 2 * @group Camera trigger From 3e27189026f9bc775317d12ec716a5bfb5484382 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 21:20:21 +0100 Subject: [PATCH 058/105] Sensor params: Flag params requiring a reboot --- src/modules/sensors/sensor_params.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 9543306cda..aa91fcf92a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2300,6 +2300,8 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); * * Set to 1000 for industry default or 900 to increase servo travel. * + * @reboot_required true + * * @min 800 * @max 1400 * @unit microseconds @@ -2316,6 +2318,8 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000); * * Set to 2000 for industry default or 2100 to increase servo travel. * + * @reboot_required true + * * @min 1600 * @max 2200 * @unit microseconds @@ -2333,6 +2337,8 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000); * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * + * @reboot_required true + * * @min 0 * @max 2200 * @unit microseconds @@ -2349,6 +2355,8 @@ PARAM_DEFINE_INT32(PWM_DISARMED, 0); * * Set to 1000 for default or 900 to increase servo travel * + * @reboot_required true + * * @min 800 * @max 1400 * @unit microseconds @@ -2365,6 +2373,8 @@ PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); * * Set to 2000 for default or 2100 to increase servo travel * + * @reboot_required true + * * @min 1600 * @max 2200 * @unit microseconds @@ -2382,6 +2392,8 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * + * @reboot_required true + * * @min 0 * @max 2200 * @unit microseconds From 0f78c28acacd292573243d1b63b4fa371e9f08d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 21:20:41 +0100 Subject: [PATCH 059/105] PX4IO: Flag params requiring a reboot --- src/drivers/px4io/px4io_params.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 51d39e97ba..bcf6cb837c 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -47,6 +47,8 @@ * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -58,6 +60,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -69,6 +73,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -80,6 +86,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -91,6 +99,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -102,6 +112,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -113,6 +125,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -124,6 +138,8 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs From 30a86f4b79281b017e1ffcea08ac65cc946d979d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Jan 2016 21:20:54 +0100 Subject: [PATCH 060/105] FMU: Flag params requiring a reboot --- src/drivers/px4fmu/px4fmu_params.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index ef345f193d..2f5461a233 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -47,6 +47,8 @@ * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -58,6 +60,8 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -69,6 +73,8 @@ PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -80,6 +86,8 @@ PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -91,6 +99,8 @@ PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs @@ -102,6 +112,8 @@ PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); * * Set to 1 to invert the channel, 0 for default direction. * + * @reboot_required true + * * @min 0 * @max 1 * @group PWM Outputs From 4437727b970282a5c2d720a1791976cfd25f839f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 00:43:23 +0100 Subject: [PATCH 061/105] Airframes XML: Generate new icons --- Tools/px4airframes/xmlout.py | 50 +++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 994fb6eeb2..841fabd4b5 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -28,29 +28,55 @@ class XMLOutput(): xml_group = ET.SubElement(xml_parameters, "airframe_group") xml_group.attrib["name"] = group.GetName() if (group.GetName() == "Standard Plane"): - xml_group.attrib["image"] = "AirframeStandardPlane.png" + xml_group.attrib["image"] = "Plane" elif (group.GetName() == "Flying Wing"): - xml_group.attrib["image"] = "AirframeFlyingWing.png" + xml_group.attrib["image"] = "FlyingWing" elif (group.GetName() == "Quadrotor x"): - xml_group.attrib["image"] = "AirframeQuadRotorX.png" + xml_group.attrib["image"] = "QuadRotorX" elif (group.GetName() == "Quadrotor +"): - xml_group.attrib["image"] = "AirframeQuadRotorPlus.png" + xml_group.attrib["image"] = "QuadRotorPlus" elif (group.GetName() == "Hexarotor x"): - xml_group.attrib["image"] = "AirframeHexaRotorX.png" + xml_group.attrib["image"] = "HexaRotorX" elif (group.GetName() == "Hexarotor +"): - xml_group.attrib["image"] = "AirframeHexaRotorPlus.png" + xml_group.attrib["image"] = "HexaRotorPlus" elif (group.GetName() == "Octorotor +"): - xml_group.attrib["image"] = "AirframeOctoRotorPlus.png" + xml_group.attrib["image"] = "OctoRotorPlus" elif (group.GetName() == "Octorotor x"): - xml_group.attrib["image"] = "AirframeOctoRotorX.png" + xml_group.attrib["image"] = "OctoRotorX" + elif (group.GetName() == "Octorotor Coaxial"): + xml_group.attrib["image"] = "OctoRotorXCoaxial" elif (group.GetName() == "Quadrotor Wide"): - xml_group.attrib["image"] = "AirframeQuadRotorH.png" + xml_group.attrib["image"] = "QuadRotorWide" elif (group.GetName() == "Quadrotor H"): - xml_group.attrib["image"] = "AirframeQuadRotorH.png" + xml_group.attrib["image"] = "QuadRotorH" elif (group.GetName() == "Simulation"): - xml_group.attrib["image"] = "AirframeSimulation.png" + xml_group.attrib["image"] = "AirframeSimulation" + elif (group.GetName() == "Plane A-Tail"): + xml_group.attrib["image"] = "PlaneATail" + elif (group.GetName() == "VTOL Duo Tailsitter"): + xml_group.attrib["image"] = "VTOLDuoRotorTailSitter" + elif (group.GetName() == "Standard VTOL"): + xml_group.attrib["image"] = "VTOLPlane" + elif (group.GetName() == "VTOL Quad Tailsitter"): + xml_group.attrib["image"] = "VTOLQuadRotorTailSitter" + elif (group.GetName() == "VTOLTiltRotor"): + xml_group.attrib["image"] = "VTOLTiltRotor" + elif (group.GetName() == "Coaxial Helicopter"): + xml_group.attrib["image"] = "HelicopterCoaxial" + elif (group.GetName() == "Hexarotor Coaxial"): + xml_group.attrib["image"] = "Y6A" + elif (group.GetName() == "Y6B"): + xml_group.attrib["image"] = "Y6B" + elif (group.GetName() == "Tricopter Y-"): + xml_group.attrib["image"] = "YMinus" + elif (group.GetName() == "Tricopter Y+"): + xml_group.attrib["image"] = "YPlus" + elif (group.GetName() == "Rover"): + xml_group.attrib["image"] = "Rover" + elif (group.GetName() == "Boat"): + xml_group.attrib["image"] = "Boat" else: - xml_group.attrib["image"] = "" + xml_group.attrib["image"] = "AirframeUnknown" for param in group.GetParams(): if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): xml_param = ET.SubElement(xml_group, "airframe") From 67bba2065e7e1f8c455cfd572cd02883f129b3dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 00:43:36 +0100 Subject: [PATCH 062/105] ROMFS: Update airframe configs --- ROMFS/px4fmu_common/init.d/13003_quad_tailsitter | 2 +- ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter | 2 +- ROMFS/px4fmu_common/init.d/2106_albatross | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter index 016f13816f..c2ac1e4e35 100644 --- a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -2,7 +2,7 @@ # # @name Quadrotor X Tailsitter # -# @type VTOL Tailsitter +# @type VTOL Quad Tailsitter # # @maintainer Roman Bapst # diff --git a/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter index dcf2fc8679..3ba4e27ea5 100644 --- a/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter @@ -2,7 +2,7 @@ # # @name Quadrotor + Tailsitter # -# @type VTOL Tailsitter +# @type VTOL Quad Tailsitter # # @maintainer Roman Bapst # diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross index 79e560fb5f..ff8809a3a4 100644 --- a/ROMFS/px4fmu_common/init.d/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -2,7 +2,7 @@ # # @name Applied Aeronautics Albatross # -# @type Standard Plane +# @type Plane A-Tail # # @output MAIN1 aileron right # @output MAIN2 aileron left From dd58dcfb9170682c3b2ba035141b95d068e83763 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 12:48:42 +0100 Subject: [PATCH 063/105] Fix CMake version check --- Tools/check_cmake.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/check_cmake.sh b/Tools/check_cmake.sh index 423b790dbd..6c9ba61a9d 100755 --- a/Tools/check_cmake.sh +++ b/Tools/check_cmake.sh @@ -6,7 +6,7 @@ then exit 1; fi -if [[ $cmake_ver == *"2.8"* ]] || [[ $cmake_ver == *"2.9"* ]] || [[ $cmake_ver == *"3.0"* ]] || [[ $cmake_ver == *"3.1"* ]] +if [[ $cmake_ver == *" 2.8"* ]] || [[ $cmake_ver == *" 2.9"* ]] || [[ $cmake_ver == *" 3.0"* ]] || [[ $cmake_ver == *" 3.1"* ]] then exit 1; fi From ee71a0d7616289812e413753e5f64bb99bfaf208 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 09:54:23 +0100 Subject: [PATCH 064/105] updated sitl_gazebo: support standard quad vtol plane --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 6af47249ca..57a54ab737 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 6af47249caefa3ddb6de2c31ae061aff1858fd0e +Subproject commit 57a54ab73740f45f7313cc8179e21cc3b0d53b41 From 960a233fe983f24beef3359700e4b41987a0926b Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 10:15:25 +0100 Subject: [PATCH 065/105] added startup script for standard vtol plane in SITL --- .../SITL/init/rcS_gazebo_standard_vtol | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 posix-configs/SITL/init/rcS_gazebo_standard_vtol diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol new file mode 100644 index 0000000000..87214919d6 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol @@ -0,0 +1,66 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 20 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set VT_TYPE 2 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set MPC_Z_VEL_MAX 1.0 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 1 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 200 -e -t -a From 2dab23ba046978dd3d5b730ee2fdc501021ab45c Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 10:19:09 +0100 Subject: [PATCH 066/105] pwm_out_sim: scale controls to correct range --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index b67ba077df..96db5d0d07 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -502,8 +502,8 @@ PWMSim::task_main() PX4_ISFINITE(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* scale for PWM output 1000 - 2000us */ + outputs.output[i] = 1500 + (500 * outputs.output[i]); } else { /* From f2e7d5ca772bef36eb5788e669d8a541fcea4803 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 10:20:35 +0100 Subject: [PATCH 067/105] simulator: support for standard vtol plane --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2c807fca05..e55899da73 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -119,7 +119,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; actuator_msg.aux2 = out[5]; - actuator_msg.aux3 = out[6]; + actuator_msg.aux3 = _actuators.output[6] > PWM_DEFAULT_MIN / 2 ? out[6] : -1.0f;; actuator_msg.aux4 = out[7]; actuator_msg.mode = 0; // need to put something here actuator_msg.nav_mode = 0; From 5fb6c75a2a38b628c867f234518fda56de56b05c Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 10:21:08 +0100 Subject: [PATCH 068/105] added mixer for standard quad vtol plane in SITL gazebo --- .../mixers/standard_vtol_sitl.main.mix | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix b/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix new file mode 100644 index 0000000000..ffa5a024bc --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix @@ -0,0 +1,24 @@ +Mixer for standard vtol plane (SITL) with motor x configuration +========================================================= + +This file defines a single mixer for a standard vtol plane (SITL gazebo) with motors in X configuration. +The plane has two ailerons and one elevator. The ailerons and elevator are treated as elevons +in order to make the standard vtol simulation compatible with the tailsitter simulation. + +R: 4x 10000 10000 10000 0 + +# mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 -5000 -5000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 5000 5000 0 -10000 10000 + +# mixer for the pusher/puller throttle +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 From 0d1872f223a4277c2d66d6e9f08893c8882c4bdf Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 13 Jan 2016 10:21:45 +0100 Subject: [PATCH 069/105] support building standard vtol plane in SITL gazebo --- Makefile | 3 ++- src/firmware/posix/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 667897ed4b..e9ab6f06e1 100644 --- a/Makefile +++ b/Makefile @@ -195,7 +195,8 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter \ + gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 2ed7c4b441..003ba63c9e 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -36,7 +36,7 @@ add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb ddd valgrind) - foreach(model none iris tailsitter) + foreach(model none iris tailsitter standard_vtol) if (debugger STREQUAL "none") if (model STREQUAL "none") set(_targ_name "${viewer}") From 05b23a8b5487ca1f729b322f2ef5a22b74deacdc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 14 Jan 2016 14:25:22 +0100 Subject: [PATCH 070/105] disable pos/vel control during mission in transitions --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfadb05e5b..6b5c29ab93 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3018,8 +3018,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; From af42f454f75ff1c2ee1a539c27519ee8bf22027d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 5 Jan 2016 16:08:57 -0700 Subject: [PATCH 071/105] fix sbus for pixracer beta --- src/drivers/boards/px4fmu-v4/board_config.h | 5 ++++- src/drivers/px4fmu/fmu.cpp | 25 ++++++++------------- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d57396a8fd..d464c8af2a 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -215,7 +215,7 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) -#define SBUS_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_PORT "/dev/ttyS4" /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -226,7 +226,10 @@ __BEGIN_DECLS #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +/* for R07, this signal is active low */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* for R12, this signal is active high */ +//#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 05d7047c97..9747cd660c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -631,19 +631,9 @@ PX4FMU::cycle() update_pwm_rev_mask(); -#ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, false); -#endif - -#ifdef DSM_SERIAL_PORT - // XXX rather than opening it we need to cycle between protocols until one is locked in - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); -#endif - _initialized = true; } - if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; @@ -811,15 +801,18 @@ PX4FMU::cycle() bool rc_updated = false; -#ifdef SBUS_SERIAL_PORT +#ifdef RC_SERIAL_PORT bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_count; - bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + + // read port + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, + &sbus_failsafe, &sbus_frame_drop, input_rc_s::RC_INPUT_MAX_CHANNELS); if (sbus_updated) { - // we have a new PPM frame. Publish it. + // we have a new SBUS frame. Publish it. _rc_in.channel_count = raw_rc_count; if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { @@ -848,8 +841,8 @@ PX4FMU::cycle() #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && - ppm_decoded_channels > 3) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) + && ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; @@ -2224,7 +2217,7 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "info")) { -#ifdef SBUS_SERIAL_PORT +#ifdef RC_SERIAL_PORT warnx("frame drops: %u", sbus_dropped_frames()); #endif return 0; From 0f3878a48a457f0d9dcda6801a6a2c9ead7e0558 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 8 Jan 2016 12:27:40 -0700 Subject: [PATCH 072/105] DSM input tested OK with DX7 --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- src/drivers/px4fmu/fmu.cpp | 62 +++++++++++++++++++-- 2 files changed, 57 insertions(+), 7 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d464c8af2a..0655868c57 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -238,7 +238,7 @@ __BEGIN_DECLS /* Power switch controls ******************************************************/ -#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s)) #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) #define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9747cd660c..d89d0cfbd4 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -134,8 +134,7 @@ private: orb_advert_t _outputs_pub; unsigned _num_outputs; int _class_instance; - int _sbus_fd; - int _dsm_fd; + int _rcs_fd; volatile bool _initialized; bool _throttle_armed; @@ -280,8 +279,7 @@ PX4FMU::PX4FMU() : _outputs_pub(nullptr), _num_outputs(0), _class_instance(0), - _sbus_fd(-1), - _dsm_fd(-1), + _rcs_fd(-1), _initialized(false), _throttle_armed(false), _pwm_on(false), @@ -801,13 +799,65 @@ PX4FMU::cycle() bool rc_updated = false; -#ifdef RC_SERIAL_PORT +#ifdef DSM_SERIAL_PORT + uint8_t n_bytes = 0; + uint8_t *bytes; + bool dsm_11_bit; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + + bool dsm_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (dsm_updated) { + + warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), dsm_updated, + raw_rc_values[0], raw_rc_values[1], raw_rc_values[2], + raw_rc_values[3]); + +// if (dsm_11_bit) { +// raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; +// +// } else { +// raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; +// } +// +// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); +// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + // we have a new SBUS frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + 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++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; +// _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); +// _rc_in.rc_failsafe = sbus_failsafe; + _rc_in.rc_lost = false; +// _rc_in.rc_lost_frame_count = sbus_dropped_frames(); + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } + +#endif + +#ifdef SBUS_SERIAL_PORT bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_count; // read port - bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, + bool sbus_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, input_rc_s::RC_INPUT_MAX_CHANNELS); From 02030d9b3608005130d2eb3cb1085d663d9b8ded Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 8 Jan 2016 17:30:03 -0700 Subject: [PATCH 073/105] scan working for SBUS and DSM --- src/drivers/px4fmu/fmu.cpp | 218 ++++++++++++++++++++++++------------- 1 file changed, 140 insertions(+), 78 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d89d0cfbd4..2633785a1e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -119,6 +119,13 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); private: + enum RC_SCAN { + RC_SCAN_SBUS = 0, + RC_SCAN_DSM, + RC_SCAN_SUMD, + RC_SCAN_ST24 + }; + static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; @@ -629,6 +636,9 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef RC_SERIAL_PORT + _rcs_fd = :: open(RC_SERIAL_PORT, O_RDWR | O_NONBLOCK); +#endif _initialized = true; } @@ -799,93 +809,145 @@ PX4FMU::cycle() bool rc_updated = false; -#ifdef DSM_SERIAL_PORT - uint8_t n_bytes = 0; - uint8_t *bytes; - bool dsm_11_bit; - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t raw_rc_count; +#ifdef RC_SERIAL_PORT + // This block scans for a supported serial RC input and locks onto the first one found + static hrt_abstime rc_scan_last_lock = 0; + static hrt_abstime rc_scan_begin = 0; +// static bool rc_scan_locked = false; + static enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; - bool dsm_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, - input_rc_s::RC_INPUT_MAX_CHANNELS); + // Scan for one second, then switch protocol + constexpr hrt_abstime rc_scan_max = 1000 * 1000; + hrt_abstime now = hrt_absolute_time(); - if (dsm_updated) { + switch (_rc_scan_state) { + case RC_SCAN_SBUS: + if (rc_scan_begin == 0) { + warnx("fmu: RC_SCAN_SBUS: begin"); + rc_scan_begin = now; +// // Configure serial port for SBUS + sbus_config(_rcs_fd, false); +// /* for PixRacer R07, this signal is active low */ +// /* for PixRacer R12, this signal is active high */ + stm32_gpiowrite(GPIO_SBUS_INV, 0); + } else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max + || hrt_absolute_time() - rc_scan_begin < rc_scan_max) { - warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), dsm_updated, - raw_rc_values[0], raw_rc_values[1], raw_rc_values[2], - raw_rc_values[3]); + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; -// if (dsm_11_bit) { -// raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + // read port + rc_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count, + &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + if (rc_updated) { + // we have a new SBUS frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + 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++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + _rc_in.rc_failsafe = sbus_failsafe; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = sbus_dropped_frames(); + _rc_in.rc_total_frame_count = 0; + + rc_scan_last_lock = now; +// rc_scan_locked = true; + } + } else { + // This triggers the port re-configuration + rc_scan_begin = 0; +// rc_scan_locked = false; + // Scan the next protocol + _rc_scan_state = RC_SCAN_DSM; + } + break; + + case RC_SCAN_DSM: + if (rc_scan_begin == 0) { + warnx("fmu: RC_SCAN_DSM: begin"); + rc_scan_begin = now; +// // Configure serial port for DSM + dsm_config(_rcs_fd); +// /* for PixRacer R07, this signal is active low */ +// /* for PixRacer R12, this signal is active high */ + stm32_gpiowrite(GPIO_SBUS_INV, 1); + } else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max + || hrt_absolute_time() - rc_scan_begin < rc_scan_max) { + + uint8_t n_bytes = 0; + uint8_t *bytes; + bool dsm_11_bit; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + + rc_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + +// warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), rc_updated, +// raw_rc_values[0], raw_rc_values[1], raw_rc_values[2], +// raw_rc_values[3]); // -// } else { -// raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; -// } -// -// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); -// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + // if (dsm_11_bit) { + // raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + // + // } else { + // raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + // } + // + // raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + // raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - // we have a new SBUS frame. Publish it. - _rc_in.channel_count = raw_rc_count; + // we have a new SBUS frame. Publish it. + _rc_in.channel_count = raw_rc_count; - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::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++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + // _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + // _rc_in.rc_failsafe = sbus_failsafe; + _rc_in.rc_lost = false; + // _rc_in.rc_lost_frame_count = sbus_dropped_frames(); + _rc_in.rc_total_frame_count = 0; + + rc_scan_last_lock = now; +// rc_scan_locked = true; + } + } else { + // This triggers the port re-configuration + rc_scan_begin = 0; +// rc_scan_locked = false; + // Scan the next protocol + _rc_scan_state = RC_SCAN_SBUS; } - - for (uint8_t i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values[i]; - } - - _rc_in.timestamp_publication = hrt_absolute_time(); - _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; - - _rc_in.rc_ppm_frame_length = 0; -// _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); -// _rc_in.rc_failsafe = sbus_failsafe; - _rc_in.rc_lost = false; -// _rc_in.rc_lost_frame_count = sbus_dropped_frames(); - _rc_in.rc_total_frame_count = 0; - - rc_updated = true; + break; + case RC_SCAN_ST24: + case RC_SCAN_SUMD: + break; } - -#endif - -#ifdef SBUS_SERIAL_PORT - bool sbus_failsafe, sbus_frame_drop; - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t raw_rc_count; - - // read port - bool sbus_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count, - &sbus_failsafe, &sbus_frame_drop, - input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (sbus_updated) { - // we have a new SBUS frame. Publish it. - _rc_in.channel_count = raw_rc_count; - - 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++) { - _rc_in.values[i] = raw_rc_values[i]; - } - - _rc_in.timestamp_publication = hrt_absolute_time(); - _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; - - _rc_in.rc_ppm_frame_length = 0; - _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); - _rc_in.rc_failsafe = sbus_failsafe; - _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = sbus_dropped_frames(); - _rc_in.rc_total_frame_count = 0; - - rc_updated = true; - } - #endif #ifdef HRT_PPM_CHANNEL From 434ce8593742ce5e5c3c193b73f73e49f703ddbb Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 8 Jan 2016 20:56:19 -0700 Subject: [PATCH 074/105] lock RC scan on first detection and fill in default values for fields not in DSM record pull serial port read out of sbus and dsm input methods clean up scanning code and add STM24 --- src/drivers/px4fmu/fmu.cpp | 278 +++++++++++++++++++++---------------- 1 file changed, 162 insertions(+), 116 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2633785a1e..4d5d36ffcf 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,7 +120,8 @@ public: private: enum RC_SCAN { - RC_SCAN_SBUS = 0, + RC_SCAN_PPM, + RC_SCAN_SBUS, RC_SCAN_DSM, RC_SCAN_SUMD, RC_SCAN_ST24 @@ -142,6 +143,7 @@ private: unsigned _num_outputs; int _class_instance; int _rcs_fd; + uint8_t _rcs_buf[SBUS_FRAME_SIZE]; volatile bool _initialized; bool _throttle_armed; @@ -205,6 +207,10 @@ private: /* do not allow to copy due to ptr data members */ PX4FMU(const PX4FMU &); PX4FMU operator=(const PX4FMU &); + void fill_rc_in(uint16_t raw_rc_count, + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, + unsigned sbus_frame_drops); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -618,6 +624,33 @@ PX4FMU::cycle_trampoline(void *arg) dev->cycle(); } +void PX4FMU::fill_rc_in(uint16_t raw_rc_count, + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, + unsigned sbus_frame_drops) +{ + // fill rc_in struct for publishing + _rc_in.channel_count = raw_rc_count; + + 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++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = now; + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = + (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + _rc_in.rc_failsafe = sbus_failsafe; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = sbus_frame_drops; + _rc_in.rc_total_frame_count = 0; +} + void PX4FMU::cycle() { @@ -638,6 +671,8 @@ PX4FMU::cycle() #ifdef RC_SERIAL_PORT _rcs_fd = :: open(RC_SERIAL_PORT, O_RDWR | O_NONBLOCK); + // assume SBUS input + sbus_config(_rcs_fd, false); #endif _initialized = true; } @@ -813,170 +848,181 @@ PX4FMU::cycle() // This block scans for a supported serial RC input and locks onto the first one found static hrt_abstime rc_scan_last_lock = 0; static hrt_abstime rc_scan_begin = 0; -// static bool rc_scan_locked = false; - static enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; + static bool rc_scan_locked = false; + static bool report_lock = true; + static enum RC_SCAN rc_scan_state = RC_SCAN_SBUS; // Scan for one second, then switch protocol constexpr hrt_abstime rc_scan_max = 1000 * 1000; - hrt_abstime now = hrt_absolute_time(); - switch (_rc_scan_state) { + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + unsigned sbus_frame_drops; + unsigned dsm_frame_drops; + bool dsm_11_bit; + + + if (report_lock && rc_scan_locked) { + report_lock = false; + warnx("fmu: RC input %u locked", rc_scan_state); + + if (rc_scan_state != RC_SCAN_PPM) { + warnx("fmu: disabling PPM input"); + // disable CPPM input by mapping it away from the timer capture input + stm32_configgpio(GPIO_PPM_IN & ~GPIO_AF_MASK); + } + } + + // read all available data from the serial RC input UART + hrt_abstime now = hrt_absolute_time(); + int newBytes = :: read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); + + switch (rc_scan_state) { case RC_SCAN_SBUS: if (rc_scan_begin == 0) { - warnx("fmu: RC_SCAN_SBUS: begin"); rc_scan_begin = now; // // Configure serial port for SBUS sbus_config(_rcs_fd, false); // /* for PixRacer R07, this signal is active low */ // /* for PixRacer R12, this signal is active high */ stm32_gpiowrite(GPIO_SBUS_INV, 0); - } else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max - || hrt_absolute_time() - rc_scan_begin < rc_scan_max) { - bool sbus_failsafe, sbus_frame_drop; - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t raw_rc_count; + } else if (now - rc_scan_last_lock < rc_scan_max + || now - rc_scan_begin < rc_scan_max) { - // read port - rc_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count, - &sbus_failsafe, &sbus_frame_drop, - input_rc_s::RC_INPUT_MAX_CHANNELS); - if (rc_updated) { - // we have a new SBUS frame. Publish it. - _rc_in.channel_count = raw_rc_count; + // parse new data + if (newBytes > 0) { + rc_updated = sbus_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, + &sbus_frame_drop, &sbus_frame_drops, input_rc_s::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; + if (rc_updated) { + // we have a new SBUS frame. Publish it. + fill_rc_in(raw_rc_count, raw_rc_values, now, + sbus_frame_drop, sbus_failsafe, sbus_frame_drops); + rc_scan_last_lock = now; + rc_scan_locked = true; } - - for (uint8_t i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values[i]; - } - - _rc_in.timestamp_publication = hrt_absolute_time(); - _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; - - _rc_in.rc_ppm_frame_length = 0; - _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); - _rc_in.rc_failsafe = sbus_failsafe; - _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = sbus_dropped_frames(); - _rc_in.rc_total_frame_count = 0; - - rc_scan_last_lock = now; -// rc_scan_locked = true; } - } else { + + } else if (!rc_scan_locked) { // This triggers the port re-configuration rc_scan_begin = 0; -// rc_scan_locked = false; // Scan the next protocol - _rc_scan_state = RC_SCAN_DSM; + rc_scan_state = RC_SCAN_DSM; } + break; case RC_SCAN_DSM: if (rc_scan_begin == 0) { - warnx("fmu: RC_SCAN_DSM: begin"); rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); // /* for PixRacer R07, this signal is active low */ // /* for PixRacer R12, this signal is active high */ stm32_gpiowrite(GPIO_SBUS_INV, 1); - } else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max - || hrt_absolute_time() - rc_scan_begin < rc_scan_max) { - uint8_t n_bytes = 0; - uint8_t *bytes; - bool dsm_11_bit; - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t raw_rc_count; + } else if (now - rc_scan_last_lock < rc_scan_max + || now - rc_scan_begin < rc_scan_max) { - rc_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, - input_rc_s::RC_INPUT_MAX_CHANNELS); + if (newBytes > 0) { + // parse new data + rc_updated = dsm_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, + &dsm_11_bit, &dsm_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); - if (rc_updated) { - -// warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), rc_updated, -// raw_rc_values[0], raw_rc_values[1], raw_rc_values[2], -// raw_rc_values[3]); -// - // if (dsm_11_bit) { - // raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - // - // } else { - // raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - // } - // - // raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - // raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - - // we have a new SBUS frame. Publish it. - _rc_in.channel_count = raw_rc_count; - - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + if (rc_updated) { + // we have a new DSM frame. Publish it. + fill_rc_in(raw_rc_count, raw_rc_values, now, + false, false, dsm_frame_drops); + rc_scan_last_lock = now; + rc_scan_locked = true; } - - for (uint8_t i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values[i]; - } - - _rc_in.timestamp_publication = hrt_absolute_time(); - _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; - - _rc_in.rc_ppm_frame_length = 0; - // _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); - // _rc_in.rc_failsafe = sbus_failsafe; - _rc_in.rc_lost = false; - // _rc_in.rc_lost_frame_count = sbus_dropped_frames(); - _rc_in.rc_total_frame_count = 0; - - rc_scan_last_lock = now; -// rc_scan_locked = true; } - } else { + + } else if (!rc_scan_locked) { // This triggers the port re-configuration rc_scan_begin = 0; -// rc_scan_locked = false; // Scan the next protocol - _rc_scan_state = RC_SCAN_SBUS; + rc_scan_state = RC_SCAN_PPM; } + break; + case RC_SCAN_ST24: + if (rc_scan_begin == 0) { + rc_scan_begin = now; +// // Configure serial port for DSM + dsm_config(_rcs_fd); +// /* for PixRacer R07, this signal is active low */ +// /* for PixRacer R12, this signal is active high */ + stm32_gpiowrite(GPIO_SBUS_INV, 1); + + } else if (now - rc_scan_last_lock < rc_scan_max + || now - rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t st24_rssi, rx_count; + + rc_updated = false; + + for (unsigned i = 0; i < newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + rc_updated |= (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count, + &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); + } + + if (rc_updated) { + // we have a new ST24 frame. Publish it. + fill_rc_in(raw_rc_count, raw_rc_values, now, + false, false, dsm_frame_drops); + rc_scan_last_lock = now; + rc_scan_locked = true; + } + } + + } else if (!rc_scan_locked) { + // This triggers the port re-configuration + rc_scan_begin = 0; + // Scan the next protocol + rc_scan_state = RC_SCAN_PPM; + } + + break; + case RC_SCAN_SUMD: break; - } -#endif -#ifdef HRT_PPM_CHANNEL + case RC_SCAN_PPM: + if (rc_scan_begin == 0) { + rc_scan_begin = now; + // Configure timer input pin for CPPM + stm32_configgpio(GPIO_PPM_IN); - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) - && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - _rc_in.channel_count = ppm_decoded_channels; + } else if (now - rc_scan_last_lock < rc_scan_max + || now - rc_scan_begin < rc_scan_max) { - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) + && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + fill_rc_in(ppm_decoded_channels, ppm_buffer, now, + false, false, 0); + rc_scan_last_lock = now; + rc_scan_locked = true; + } + + } else if (!rc_scan_locked) { + // This triggers the port re-configuration + rc_scan_begin = 0; + // Scan the next protocol + rc_scan_state = RC_SCAN_SBUS; } - for (uint8_t i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = ppm_buffer[i]; - } - - _rc_in.timestamp_publication = ppm_last_valid_decode; - _rc_in.timestamp_last_signal = ppm_last_valid_decode; - - _rc_in.rc_ppm_frame_length = ppm_frame_length; - _rc_in.rssi = RC_INPUT_RSSI_MAX; - _rc_in.rc_failsafe = false; - _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = 0; - _rc_in.rc_total_frame_count = 0; - - rc_updated = true; + break; } #endif From 71a3e3713c3d82113f5f1b0ca7dc8a0cfa112fa2 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 10 Jan 2016 08:17:23 -0700 Subject: [PATCH 075/105] move PPM input disable into scan case for PPM; PPM input is now enabled only while in RC_SCAN_PPM state --- src/drivers/px4fmu/fmu.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d5d36ffcf..8a20814966 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -673,6 +673,8 @@ PX4FMU::cycle() _rcs_fd = :: open(RC_SERIAL_PORT, O_RDWR | O_NONBLOCK); // assume SBUS input sbus_config(_rcs_fd, false); + // disable CPPM input by mapping it away from the timer capture input + stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); #endif _initialized = true; } @@ -866,12 +868,6 @@ PX4FMU::cycle() if (report_lock && rc_scan_locked) { report_lock = false; warnx("fmu: RC input %u locked", rc_scan_state); - - if (rc_scan_state != RC_SCAN_PPM) { - warnx("fmu: disabling PPM input"); - // disable CPPM input by mapping it away from the timer capture input - stm32_configgpio(GPIO_PPM_IN & ~GPIO_AF_MASK); - } } // read all available data from the serial RC input UART @@ -1018,6 +1014,8 @@ PX4FMU::cycle() } else if (!rc_scan_locked) { // This triggers the port re-configuration rc_scan_begin = 0; + // disable CPPM input by mapping it away from the timer capture input + stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); // Scan the next protocol rc_scan_state = RC_SCAN_SBUS; } From 72156d9cd6edf467887ac6d1c33c7208031f1f05 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 10 Jan 2016 09:28:46 -0700 Subject: [PATCH 076/105] add macro to control RC input inverter remove redundant variable --- src/drivers/boards/px4fmu-v4/board_config.h | 2 + src/drivers/px4fmu/fmu.cpp | 47 ++++++++++----------- 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 0655868c57..03afcda0b4 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -228,8 +228,10 @@ __BEGIN_DECLS #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) /* for R07, this signal is active low */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, 1-_s); /* for R12, this signal is active high */ //#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +//#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, _s); #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a20814966..2a7978f347 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -209,8 +209,8 @@ private: PX4FMU operator=(const PX4FMU &); void fill_rc_in(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, - unsigned sbus_frame_drops); + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -626,8 +626,8 @@ PX4FMU::cycle_trampoline(void *arg) void PX4FMU::fill_rc_in(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, - unsigned sbus_frame_drops) + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi = -1) { // fill rc_in struct for publishing _rc_in.channel_count = raw_rc_count; @@ -643,11 +643,15 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, _rc_in.timestamp_publication = now; _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; _rc_in.rc_ppm_frame_length = 0; - _rc_in.rssi = - (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); - _rc_in.rc_failsafe = sbus_failsafe; + + if (rssi == -1) { + _rc_in.rssi = + (!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + } + + _rc_in.rc_failsafe = failsafe; _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = sbus_frame_drops; + _rc_in.rc_lost_frame_count = frame_drops; _rc_in.rc_total_frame_count = 0; } @@ -860,8 +864,7 @@ PX4FMU::cycle() bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_count; - unsigned sbus_frame_drops; - unsigned dsm_frame_drops; + unsigned frame_drops; bool dsm_11_bit; @@ -878,11 +881,9 @@ PX4FMU::cycle() case RC_SCAN_SBUS: if (rc_scan_begin == 0) { rc_scan_begin = now; -// // Configure serial port for SBUS + // Configure serial port for SBUS sbus_config(_rcs_fd, false); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 0); + INVERT_RC_INPUT(true); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -890,12 +891,12 @@ PX4FMU::cycle() // parse new data if (newBytes > 0) { rc_updated = sbus_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, - &sbus_frame_drop, &sbus_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - sbus_frame_drop, sbus_failsafe, sbus_frame_drops); + sbus_frame_drop, sbus_failsafe, frame_drops); rc_scan_last_lock = now; rc_scan_locked = true; } @@ -915,9 +916,7 @@ PX4FMU::cycle() rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 1); + INVERT_RC_INPUT(false); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -925,12 +924,12 @@ PX4FMU::cycle() if (newBytes > 0) { // parse new data rc_updated = dsm_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &dsm_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new DSM frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - false, false, dsm_frame_drops); + false, false, frame_drops); rc_scan_last_lock = now; rc_scan_locked = true; } @@ -950,9 +949,7 @@ PX4FMU::cycle() rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 1); + INVERT_RC_INPUT(false); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -973,7 +970,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new ST24 frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - false, false, dsm_frame_drops); + false, false, frame_drops, st24_rssi); rc_scan_last_lock = now; rc_scan_locked = true; } From ca2e9e7be1634973a041979f76bd34cf8dfacb0f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 10 Jan 2016 11:41:53 -0700 Subject: [PATCH 077/105] handle PPM input with RC_SERIAL_PORT undefined --- src/drivers/px4fmu/fmu.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2a7978f347..d1cda94b4b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -939,7 +939,7 @@ PX4FMU::cycle() // This triggers the port re-configuration rc_scan_begin = 0; // Scan the next protocol - rc_scan_state = RC_SCAN_PPM; + rc_scan_state = RC_SCAN_ST24; } break; @@ -980,15 +980,18 @@ PX4FMU::cycle() // This triggers the port re-configuration rc_scan_begin = 0; // Scan the next protocol - rc_scan_state = RC_SCAN_PPM; + rc_scan_state = RC_SCAN_SUMD; } break; case RC_SCAN_SUMD: + rc_scan_state = RC_SCAN_PPM; break; case RC_SCAN_PPM: + // skip PPM if it's not supported +#ifdef HRT_PPM_CHANNEL if (rc_scan_begin == 0) { rc_scan_begin = now; // Configure timer input pin for CPPM @@ -1017,10 +1020,28 @@ PX4FMU::cycle() rc_scan_state = RC_SCAN_SBUS; } +#else // skip PPM if it's not supported + rc_scan_state = RC_SCAN_SBUS; + +#endif // HRT_PPM_CHANNEL + break; } -#endif +#else // RC_SERIAL_PORT not defined +#ifdef HRT_PPM_CHANNEL + + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) + && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + fill_rc_in(ppm_decoded_channels, ppm_buffer, hrt_absolute_time(), + false, false, 0); + } + +#endif // HRT_PPM_CHANNEL +#endif // RC_SERIAL_PORT if (rc_updated) { /* lazily advertise on first publication */ From 425169921c5f1205ae914aeb56946b8785a48d58 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 10 Jan 2016 18:01:30 -0700 Subject: [PATCH 078/105] begin adding DSM bind function --- src/drivers/px4fmu/fmu.cpp | 70 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 68 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d1cda94b4b..e4b0ee37c1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -211,6 +211,7 @@ private: uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi); + void dsm_bind_ioctl(int dsmMode); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -674,7 +675,8 @@ PX4FMU::cycle() update_pwm_rev_mask(); #ifdef RC_SERIAL_PORT - _rcs_fd = :: open(RC_SERIAL_PORT, O_RDWR | O_NONBLOCK); + // dsm_init sets some file static variables and returns a file descriptor + _rcs_fd = dsm_init(RC_SERIAL_PORT); // assume SBUS input sbus_config(_rcs_fd, false); // disable CPPM input by mapping it away from the timer capture input @@ -846,6 +848,18 @@ PX4FMU::cycle() orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); update_pwm_rev_mask(); + + int32_t dsm_bind_val; + param_t dsm_bind_param; + + /* see if bind parameter has been set, and reset it to -1 */ + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + + if (dsm_bind_val > -1) { + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; + param_set(dsm_bind_param, &dsm_bind_val); + } } bool rc_updated = false; @@ -875,7 +889,7 @@ PX4FMU::cycle() // read all available data from the serial RC input UART hrt_abstime now = hrt_absolute_time(); - int newBytes = :: read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); + int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); switch (rc_scan_state) { case RC_SCAN_SBUS: @@ -1530,6 +1544,38 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#ifdef RC_SERIAL_PORT + + case DSM_BIND_START: + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + warnx("fmu pwm_ioctl: DSM_BIND_START, arg: %lu", arg); + + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + + dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0); + usleep(500000); + + dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0); + + dsm_bind(DSM_CMD_BIND_POWER_UP, 0); + usleep(72000); + + dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg); + usleep(50000); + + dsm_bind(DSM_CMD_BIND_REINIT_UART, 0); + + ret = OK; + + } else { + ret = -EINVAL; + } + + break; +#endif + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1998,6 +2044,26 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } +void +PX4FMU::dsm_bind_ioctl(int dsmMode) +{ + if (!_armed.armed) { +// mavlink_log_info(_mavlink_fd, "[FMU] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); + warnx("[FMU] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); + int ret = ioctl(nullptr, DSM_BIND_START, + (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + + if (ret) { +// mavlink_log_critical(_mavlink_fd, "binding failed."); + warnx("binding failed."); + } + + } else { +// mavlink_log_info(_mavlink_fd, "[FMU] system armed, bind request rejected"); + warnx("[FMU] system armed, bind request rejected"); + } +} + namespace { From 9eecca6a719674c5e78a9e353fb73df254b6848e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 11 Jan 2016 10:26:58 -0700 Subject: [PATCH 079/105] add string value struct for RC_SCAN enum --- src/drivers/px4fmu/fmu.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e4b0ee37c1..f78eae57e2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,12 +120,19 @@ public: private: enum RC_SCAN { - RC_SCAN_PPM, + RC_SCAN_PPM = 0, RC_SCAN_SBUS, RC_SCAN_DSM, RC_SCAN_SUMD, RC_SCAN_ST24 }; + const char* RC_SCAN_STRING[5] = { + "PPM", + "SBUS", + "DSM", + "SUMD", + "ST24" + }; static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; @@ -682,7 +689,8 @@ PX4FMU::cycle() // disable CPPM input by mapping it away from the timer capture input stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); #endif - _initialized = true; + + _initialized = true; } if (_groups_subscribed != _groups_required) { @@ -884,7 +892,7 @@ PX4FMU::cycle() if (report_lock && rc_scan_locked) { report_lock = false; - warnx("fmu: RC input %u locked", rc_scan_state); + warnx("fmu: %s RC input locked", RC_SCAN_STRING[rc_scan_state]); } // read all available data from the serial RC input UART From eb36eac13789e79c3771bdafd00277a03e1d7612 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 11 Jan 2016 14:47:43 -0700 Subject: [PATCH 080/105] reduce scan interval to 100msec --- src/drivers/px4fmu/fmu.cpp | 117 +++++++++++++++++++------------------ 1 file changed, 59 insertions(+), 58 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f78eae57e2..812c519cf2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -119,14 +119,16 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); private: - enum RC_SCAN { + enum RC_SCAN { RC_SCAN_PPM = 0, RC_SCAN_SBUS, RC_SCAN_DSM, RC_SCAN_SUMD, RC_SCAN_ST24 }; - const char* RC_SCAN_STRING[5] = { + enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; + + char const * RC_SCAN_STRING[5] = { "PPM", "SBUS", "DSM", @@ -134,7 +136,12 @@ private: "ST24" }; - static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; + hrt_abstime _rc_scan_last_lock = 0; + hrt_abstime _rc_scan_begin = 0; + bool _rc_scan_locked = false; + bool _report_lock = true; + + static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; unsigned _pwm_default_rate; @@ -219,6 +226,7 @@ private: hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi); void dsm_bind_ioctl(int dsmMode); + void set_rc_scan_state(RC_SCAN _rc_scan_state); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -663,6 +671,13 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, _rc_in.rc_total_frame_count = 0; } +void PX4FMU::set_rc_scan_state(RC_SCAN newState) +{ + warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); + _rc_scan_begin = 0; + _rc_scan_state = newState; +} + void PX4FMU::cycle() { @@ -874,14 +889,8 @@ PX4FMU::cycle() #ifdef RC_SERIAL_PORT // This block scans for a supported serial RC input and locks onto the first one found - static hrt_abstime rc_scan_last_lock = 0; - static hrt_abstime rc_scan_begin = 0; - static bool rc_scan_locked = false; - static bool report_lock = true; - static enum RC_SCAN rc_scan_state = RC_SCAN_SBUS; - - // Scan for one second, then switch protocol - constexpr hrt_abstime rc_scan_max = 1000 * 1000; + // Scan for 100 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 100 * 1000; bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; @@ -890,25 +899,25 @@ PX4FMU::cycle() bool dsm_11_bit; - if (report_lock && rc_scan_locked) { - report_lock = false; - warnx("fmu: %s RC input locked", RC_SCAN_STRING[rc_scan_state]); + if (_report_lock && _rc_scan_locked) { + _report_lock = false; + warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); } // read all available data from the serial RC input UART hrt_abstime now = hrt_absolute_time(); int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); - switch (rc_scan_state) { + switch (_rc_scan_state) { case RC_SCAN_SBUS: - if (rc_scan_begin == 0) { - rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // Configure serial port for SBUS sbus_config(_rcs_fd, false); INVERT_RC_INPUT(true); - } else if (now - rc_scan_last_lock < rc_scan_max - || now - rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { // parse new data if (newBytes > 0) { @@ -919,29 +928,27 @@ PX4FMU::cycle() // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, sbus_frame_drop, sbus_failsafe, frame_drops); - rc_scan_last_lock = now; - rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!rc_scan_locked) { - // This triggers the port re-configuration - rc_scan_begin = 0; + } else if (!_rc_scan_locked) { // Scan the next protocol - rc_scan_state = RC_SCAN_DSM; + set_rc_scan_state(RC_SCAN_DSM); } break; case RC_SCAN_DSM: - if (rc_scan_begin == 0) { - rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); INVERT_RC_INPUT(false); - } else if (now - rc_scan_last_lock < rc_scan_max - || now - rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { // parse new data @@ -952,29 +959,27 @@ PX4FMU::cycle() // we have a new DSM frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops); - rc_scan_last_lock = now; - rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!rc_scan_locked) { - // This triggers the port re-configuration - rc_scan_begin = 0; + } else if (!_rc_scan_locked) { // Scan the next protocol - rc_scan_state = RC_SCAN_ST24; + set_rc_scan_state(RC_SCAN_ST24); } break; case RC_SCAN_ST24: - if (rc_scan_begin == 0) { - rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); INVERT_RC_INPUT(false); - } else if (now - rc_scan_last_lock < rc_scan_max - || now - rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { // parse new data @@ -993,34 +998,32 @@ PX4FMU::cycle() // we have a new ST24 frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, st24_rssi); - rc_scan_last_lock = now; - rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!rc_scan_locked) { - // This triggers the port re-configuration - rc_scan_begin = 0; + } else if (!_rc_scan_locked) { // Scan the next protocol - rc_scan_state = RC_SCAN_SUMD; + set_rc_scan_state(RC_SCAN_SUMD); } break; case RC_SCAN_SUMD: - rc_scan_state = RC_SCAN_PPM; - break; + set_rc_scan_state(RC_SCAN_PPM); + break; case RC_SCAN_PPM: // skip PPM if it's not supported #ifdef HRT_PPM_CHANNEL - if (rc_scan_begin == 0) { - rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // Configure timer input pin for CPPM stm32_configgpio(GPIO_PPM_IN); - } else if (now - rc_scan_last_lock < rc_scan_max - || now - rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { // see if we have new PPM input data if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) @@ -1029,21 +1032,19 @@ PX4FMU::cycle() rc_updated = true; fill_rc_in(ppm_decoded_channels, ppm_buffer, now, false, false, 0); - rc_scan_last_lock = now; - rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } - } else if (!rc_scan_locked) { - // This triggers the port re-configuration - rc_scan_begin = 0; + } else if (!_rc_scan_locked) { // disable CPPM input by mapping it away from the timer capture input stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); // Scan the next protocol - rc_scan_state = RC_SCAN_SBUS; + set_rc_scan_state(RC_SCAN_SBUS); } #else // skip PPM if it's not supported - rc_scan_state = RC_SCAN_SBUS; + set_rc_scan_state(RC_SCAN_SBUS); #endif // HRT_PPM_CHANNEL From 3d185e18e966576fb5fb18252fd1caf9ed5b26b6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 11 Jan 2016 14:49:41 -0700 Subject: [PATCH 081/105] remove warnx in set_rc_scan_state --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 812c519cf2..c26c487e9e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -673,7 +673,7 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, void PX4FMU::set_rc_scan_state(RC_SCAN newState) { - warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); +// warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); _rc_scan_begin = 0; _rc_scan_state = newState; } From e0bbbd356fd1a4656fc0f7e0b68aa1ec5f478b2f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 11 Jan 2016 15:11:34 -0700 Subject: [PATCH 082/105] add SUMD decoder to RCscan --- src/drivers/px4fmu/fmu.cpp | 160 +++++++++++++++++++++++-------------- 1 file changed, 98 insertions(+), 62 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c26c487e9e..8d72c42bd0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -119,29 +119,29 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); private: - enum RC_SCAN { - RC_SCAN_PPM = 0, + enum RC_SCAN { + RC_SCAN_PPM = 0, RC_SCAN_SBUS, RC_SCAN_DSM, RC_SCAN_SUMD, RC_SCAN_ST24 }; - enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; + enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; - char const * RC_SCAN_STRING[5] = { - "PPM", - "SBUS", - "DSM", - "SUMD", - "ST24" - }; + char const *RC_SCAN_STRING[5] = { + "PPM", + "SBUS", + "DSM", + "SUMD", + "ST24" + }; - hrt_abstime _rc_scan_last_lock = 0; - hrt_abstime _rc_scan_begin = 0; - bool _rc_scan_locked = false; - bool _report_lock = true; + hrt_abstime _rc_scan_last_lock = 0; + hrt_abstime _rc_scan_begin = 0; + bool _rc_scan_locked = false; + bool _report_lock = true; - static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; + static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; unsigned _pwm_default_rate; @@ -226,7 +226,7 @@ private: hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi); void dsm_bind_ioctl(int dsmMode); - void set_rc_scan_state(RC_SCAN _rc_scan_state); + void set_rc_scan_state(RC_SCAN _rc_scan_state); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -674,8 +674,8 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, void PX4FMU::set_rc_scan_state(RC_SCAN newState) { // warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); - _rc_scan_begin = 0; - _rc_scan_state = newState; + _rc_scan_begin = 0; + _rc_scan_state = newState; } void @@ -705,7 +705,7 @@ PX4FMU::cycle() stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); #endif - _initialized = true; + _initialized = true; } if (_groups_subscribed != _groups_required) { @@ -889,8 +889,8 @@ PX4FMU::cycle() #ifdef RC_SERIAL_PORT // This block scans for a supported serial RC input and locks onto the first one found - // Scan for 100 msec, then switch protocol - constexpr hrt_abstime rc_scan_max = 100 * 1000; + // Scan for 100 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 100 * 1000; bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; @@ -899,25 +899,25 @@ PX4FMU::cycle() bool dsm_11_bit; - if (_report_lock && _rc_scan_locked) { - _report_lock = false; - warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + if (_report_lock && _rc_scan_locked) { + _report_lock = false; + warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); } // read all available data from the serial RC input UART hrt_abstime now = hrt_absolute_time(); int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); - switch (_rc_scan_state) { + switch (_rc_scan_state) { case RC_SCAN_SBUS: - if (_rc_scan_begin == 0) { - _rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // Configure serial port for SBUS sbus_config(_rcs_fd, false); INVERT_RC_INPUT(true); - } else if (now - _rc_scan_last_lock < rc_scan_max - || now - _rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { // parse new data if (newBytes > 0) { @@ -928,27 +928,27 @@ PX4FMU::cycle() // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_last_lock = now; - _rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else if (!_rc_scan_locked) { // Scan the next protocol - set_rc_scan_state(RC_SCAN_DSM); + set_rc_scan_state(RC_SCAN_DSM); } break; case RC_SCAN_DSM: - if (_rc_scan_begin == 0) { - _rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); INVERT_RC_INPUT(false); - } else if (now - _rc_scan_last_lock < rc_scan_max - || now - _rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { // parse new data @@ -959,27 +959,27 @@ PX4FMU::cycle() // we have a new DSM frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops); - _rc_scan_last_lock = now; - _rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else if (!_rc_scan_locked) { // Scan the next protocol - set_rc_scan_state(RC_SCAN_ST24); + set_rc_scan_state(RC_SCAN_ST24); } break; case RC_SCAN_ST24: - if (_rc_scan_begin == 0) { - _rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); INVERT_RC_INPUT(false); - } else if (now - _rc_scan_last_lock < rc_scan_max - || now - _rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { // parse new data @@ -990,40 +990,76 @@ PX4FMU::cycle() for (unsigned i = 0; i < newBytes; i++) { /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; - rc_updated |= (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count, - &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); + rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count, + &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); } if (rc_updated) { // we have a new ST24 frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, st24_rssi); - _rc_scan_last_lock = now; - _rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else if (!_rc_scan_locked) { // Scan the next protocol - set_rc_scan_state(RC_SCAN_SUMD); + set_rc_scan_state(RC_SCAN_SUMD); } break; case RC_SCAN_SUMD: - set_rc_scan_state(RC_SCAN_PPM); - break; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; +// // Configure serial port for DSM + dsm_config(_rcs_fd); + INVERT_RC_INPUT(false); + + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t sumd_rssi, rx_count; + + rc_updated = false; + + for (unsigned i = 0; i < newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + sumd_rssi = RC_INPUT_RSSI_MAX; + rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, + &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); + } + + if (rc_updated) { + // we have a new SUMD frame. Publish it. + fill_rc_in(raw_rc_count, raw_rc_values, now, + false, false, frame_drops, sumd_rssi); + _rc_scan_last_lock = now; + _rc_scan_locked = true; + } + } + + } else if (!_rc_scan_locked) { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SUMD); + } + + set_rc_scan_state(RC_SCAN_PPM); + break; case RC_SCAN_PPM: // skip PPM if it's not supported #ifdef HRT_PPM_CHANNEL - if (_rc_scan_begin == 0) { - _rc_scan_begin = now; + if (_rc_scan_begin == 0) { + _rc_scan_begin = now; // Configure timer input pin for CPPM stm32_configgpio(GPIO_PPM_IN); - } else if (now - _rc_scan_last_lock < rc_scan_max - || now - _rc_scan_begin < rc_scan_max) { + } else if (now - _rc_scan_last_lock < rc_scan_max + || now - _rc_scan_begin < rc_scan_max) { // see if we have new PPM input data if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) @@ -1032,19 +1068,19 @@ PX4FMU::cycle() rc_updated = true; fill_rc_in(ppm_decoded_channels, ppm_buffer, now, false, false, 0); - _rc_scan_last_lock = now; - _rc_scan_locked = true; + _rc_scan_last_lock = now; + _rc_scan_locked = true; } - } else if (!_rc_scan_locked) { + } else if (!_rc_scan_locked) { // disable CPPM input by mapping it away from the timer capture input stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); // Scan the next protocol - set_rc_scan_state(RC_SCAN_SBUS); + set_rc_scan_state(RC_SCAN_SBUS); } #else // skip PPM if it's not supported - set_rc_scan_state(RC_SCAN_SBUS); + set_rc_scan_state(RC_SCAN_SBUS); #endif // HRT_PPM_CHANNEL From 5cf78cd450b777a557ffc4371f7a154f694e9205 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 11 Jan 2016 21:14:20 -0700 Subject: [PATCH 083/105] configure usart6 TX for RC out add RC_OUT pin to FMUv4 config --- nuttx-configs/px4fmu-v4/include/board.h | 2 +- src/drivers/boards/px4fmu-v4/board_config.h | 11 +++++++---- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 4 ++++ 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 2f7c1189f3..1eb9fc9146 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -215,7 +215,7 @@ #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 -#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* RC_INPUT */ #define GPIO_USART6_TX GPIO_USART6_TX_1 #define GPIO_UART7_RX GPIO_UART7_RX_1 diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 03afcda0b4..65d0aa4532 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -240,11 +240,14 @@ __BEGIN_DECLS /* Power switch controls ******************************************************/ -#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s)) -#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) -#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s)) +//#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) -#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) + +// FMUv4 has a separate GPIO for serial RC output +#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_RC_OUT) +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_RC_OUT, (_s)) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index d093e47bf4..12f107b308 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -233,6 +233,10 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_8266_PD); stm32_configgpio(GPIO_8266_RST); +#ifdef GPIO_RC_OUT + stm32_configgpio(GPIO_RC_OUT); /* Serial RC output pin */ + stm32_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */ +#endif /* configure the high-resolution time/callout interface */ hrt_init(); From 47207b8fc8d874639e1ca3ab565d6c7da9c982b2 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 12 Jan 2016 16:44:38 -0700 Subject: [PATCH 084/105] set FMU_RC_OUTPUT high for all non-SBUS modes --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- src/drivers/px4fmu/fmu.cpp | 21 +++++++++++++++++---- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 65d0aa4532..f5ba9bb8df 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -245,7 +245,7 @@ __BEGIN_DECLS #define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) // FMUv4 has a separate GPIO for serial RC output -#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) +#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) #define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_RC_OUT) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_RC_OUT, (_s)) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8d72c42bd0..d8e156351b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -227,6 +227,8 @@ private: unsigned frame_drops, int rssi); void dsm_bind_ioctl(int dsmMode); void set_rc_scan_state(RC_SCAN _rc_scan_state); + void rc_io_invert(); + void rc_io_invert(bool invert); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -678,6 +680,16 @@ void PX4FMU::set_rc_scan_state(RC_SCAN newState) _rc_scan_state = newState; } +void PX4FMU::rc_io_invert(bool invert) +{ + INVERT_RC_INPUT(invert); + + if (!invert) { + // set FMU_RC_OUTPUT high to pull RC_INPUT up + stm32_gpiowrite(GPIO_RC_OUT, 1); + } +} + void PX4FMU::cycle() { @@ -914,7 +926,7 @@ PX4FMU::cycle() _rc_scan_begin = now; // Configure serial port for SBUS sbus_config(_rcs_fd, false); - INVERT_RC_INPUT(true); + rc_io_invert(true); } else if (now - _rc_scan_last_lock < rc_scan_max || now - _rc_scan_begin < rc_scan_max) { @@ -945,7 +957,7 @@ PX4FMU::cycle() _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); - INVERT_RC_INPUT(false); + rc_io_invert(false); } else if (now - _rc_scan_last_lock < rc_scan_max || now - _rc_scan_begin < rc_scan_max) { @@ -976,7 +988,7 @@ PX4FMU::cycle() _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); - INVERT_RC_INPUT(false); + rc_io_invert(false); } else if (now - _rc_scan_last_lock < rc_scan_max || now - _rc_scan_begin < rc_scan_max) { @@ -1015,7 +1027,7 @@ PX4FMU::cycle() _rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); - INVERT_RC_INPUT(false); + rc_io_invert(false); } else if (now - _rc_scan_last_lock < rc_scan_max || now - _rc_scan_begin < rc_scan_max) { @@ -1057,6 +1069,7 @@ PX4FMU::cycle() _rc_scan_begin = now; // Configure timer input pin for CPPM stm32_configgpio(GPIO_PPM_IN); + rc_io_invert(false); } else if (now - _rc_scan_last_lock < rc_scan_max || now - _rc_scan_begin < rc_scan_max) { From 32626b57a42f06fbd2d0a17532590c2b9906a2cc Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 12 Jan 2016 20:58:53 -0700 Subject: [PATCH 085/105] add missing #ifdef --- src/drivers/px4fmu/fmu.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d8e156351b..0a629bb54f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -673,6 +673,7 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, _rc_in.rc_total_frame_count = 0; } +#ifdef RC_SERIAL_PORT void PX4FMU::set_rc_scan_state(RC_SCAN newState) { // warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); @@ -689,6 +690,7 @@ void PX4FMU::rc_io_invert(bool invert) stm32_gpiowrite(GPIO_RC_OUT, 1); } } +#endif void PX4FMU::cycle() From 4952d056520786a0932974c593c894a6ea30354d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 31 Dec 2015 17:47:31 +0200 Subject: [PATCH 086/105] debugging ms5611 on spi2 --- CMakeLists.txt | 4 ++-- nuttx-configs/px4fmu-v4/include/board.h | 3 ++- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 4 ++-- src/drivers/ms5611/ms5611_nuttx.cpp | 8 +++++++- src/drivers/ms5611/ms5611_spi.cpp | 6 ++++++ 6 files changed, 20 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9bd26e839c..dcb33b67e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,10 +132,10 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) set(CMAKE_BUILD_TYPE "" CACHE STRING "build type") set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ";Debug;Release;RelWithDebInfo;MinSizeRel") -set(CONFIG "nuttx_px4fmu-v2_default" CACHE STRING "desired configuration") +set(CONFIG "nuttx_px4fmu-v4_default" CACHE STRING "desired configuration") file(GLOB_RECURSE configs RELATIVE cmake/configs "cmake/configs/*.cmake") set_property(CACHE CONFIG PROPERTY STRINGS ${configs}) -set(THREADS "4" CACHE STRING +set(THREADS "8" CACHE STRING "number of threads to use for external build processes") set(DEBUG_PORT "/dev/ttyACM0" CACHE STRING "debugging port") diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 1eb9fc9146..f882795bb3 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -257,9 +257,10 @@ #define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + #define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_1|GPIO_SPEED_50MHz) /************************************************************************************ * Public Data diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index f5ba9bb8df..7e554c2a74 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -73,7 +73,7 @@ __BEGIN_DECLS /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) #define GPIO_LED_RED GPIO_LED1 diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 12f107b308..d98b629d74 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -288,7 +288,6 @@ __EXPORT int nsh_archinitialize(void) SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); - SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); @@ -306,10 +305,11 @@ __EXPORT int nsh_archinitialize(void) * and de-assert the known chip selects. */ // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated - SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETFREQUENCY(spi2, 1 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); + SPI_SELECT(spi2, PX4_SPIDEV_BARO, false); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 91dd785bc6..8c950b5df8 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -829,7 +829,7 @@ struct ms5611_bus_option { { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, #endif #ifdef PX4_SPIDEV_BARO - { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_RAMTRON, NULL }, #endif #ifdef PX4_I2C_BUS_ONBOARD { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, @@ -902,6 +902,8 @@ crc4(uint16_t *n_prom) bool start_bus(struct ms5611_bus_option &bus) { + warnx("start_bus id %u", (unsigned)bus.busid); + if (bus.dev != nullptr) { errx(1, "bus option already started"); } @@ -909,6 +911,8 @@ start_bus(struct ms5611_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + warnx("init ms5611 on bus id %u", (unsigned)bus.busid); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); @@ -923,6 +927,8 @@ start_bus(struct ms5611_bus_option &bus) return false; } + warnx("open ms5611 on bus %u", (unsigned)bus.busid); + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 78192c3a98..9cb6880201 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -149,6 +149,7 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("SPI init failed"); + warnx("SPI::init failed"); goto out; } @@ -157,6 +158,7 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("reset failed"); + warnx("SPI::reset failed"); goto out; } @@ -165,6 +167,7 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("prom readout failed"); + warnx("SPI::prom readout failed"); goto out; } @@ -261,6 +264,7 @@ MS5611_SPI::_read_prom() } //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); + warnx("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ @@ -268,10 +272,12 @@ MS5611_SPI::_read_prom() if (ret != OK) { DEVICE_DEBUG("crc failed"); + warnx("crc failed"); } if (all_zero) { DEVICE_DEBUG("prom all zero"); + warnx("prom all zero"); ret = -EIO; } From ab71af60534b854409d4c992679db26a9089315c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 31 Dec 2015 21:57:21 +0200 Subject: [PATCH 087/105] fix spi select logic --- src/drivers/boards/px4fmu-v4/board_config.h | 1 + src/drivers/boards/px4fmu-v4/px4fmu_spi.c | 20 ++++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 7e554c2a74..35350e1570 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -122,6 +122,7 @@ __BEGIN_DECLS #define PX4_SPI_BUS_RAMTRON 2 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +//#define PX4_SPIDEV_FRAM 0 #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index bdfa47f62c..30560b717f 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -151,8 +151,24 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi #ifdef CONFIG_STM32_SPI2 __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case SPIDEV_FLASH: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + break; + + default: + break; + } } __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) From ecbcfe838bcf002c2b749bb4d22931e751c3e94a Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 31 Dec 2015 15:38:35 -0700 Subject: [PATCH 088/105] clean up, change SPI2 clock back to 12MHz and remove debug prints --- src/drivers/boards/px4fmu-v4/board_config.h | 8 ++++++-- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 9 +++++---- src/drivers/ms5611/ms5611_nuttx.cpp | 6 ------ src/drivers/ms5611/ms5611_spi.cpp | 6 ------ 4 files changed, 11 insertions(+), 18 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 35350e1570..733b18aa50 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -122,14 +122,18 @@ __BEGIN_DECLS #define PX4_SPI_BUS_RAMTRON 2 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ -//#define PX4_SPIDEV_FRAM 0 #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 -#define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 #define PX4_SPIDEV_ICM 6 +/* onboard MS5611 and FRAM are both on bus SPI2 + * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver + * use 3 for the barometer to differentiate + */ +#define PX4_SPIDEV_BARO 3 + /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index d98b629d74..f70f5e3725 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -301,11 +301,12 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) - * and de-assert the known chip selects. */ + /* Default SPI2 to 12MHz and de-assert the known chip selects. + * MS5611 has max SPI clock speed of 20MHz + */ - // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated - SPI_SETFREQUENCY(spi2, 1 * 1000 * 1000); + // XXX start with 10.4 MHz and go up to 20 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 8c950b5df8..bc4ac4768d 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -902,8 +902,6 @@ crc4(uint16_t *n_prom) bool start_bus(struct ms5611_bus_option &bus) { - warnx("start_bus id %u", (unsigned)bus.busid); - if (bus.dev != nullptr) { errx(1, "bus option already started"); } @@ -911,8 +909,6 @@ start_bus(struct ms5611_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); - warnx("init ms5611 on bus id %u", (unsigned)bus.busid); - if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); @@ -927,8 +923,6 @@ start_bus(struct ms5611_bus_option &bus) return false; } - warnx("open ms5611 on bus %u", (unsigned)bus.busid); - int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 9cb6880201..78192c3a98 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -149,7 +149,6 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("SPI init failed"); - warnx("SPI::init failed"); goto out; } @@ -158,7 +157,6 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("reset failed"); - warnx("SPI::reset failed"); goto out; } @@ -167,7 +165,6 @@ MS5611_SPI::init() if (ret != OK) { DEVICE_DEBUG("prom readout failed"); - warnx("SPI::prom readout failed"); goto out; } @@ -264,7 +261,6 @@ MS5611_SPI::_read_prom() } //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); - warnx("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ @@ -272,12 +268,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { DEVICE_DEBUG("crc failed"); - warnx("crc failed"); } if (all_zero) { DEVICE_DEBUG("prom all zero"); - warnx("prom all zero"); ret = -EIO; } From df5cb5472d6867eb280ff12158d7969cb3f1e8dd Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 13 Jan 2016 06:37:37 -0700 Subject: [PATCH 089/105] configure GPIO_SBUS_INV for PixRacer R12 --- src/drivers/boards/px4fmu-v4/board_config.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 733b18aa50..a2a6562aa7 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -232,11 +232,11 @@ __BEGIN_DECLS #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) /* for R07, this signal is active low */ -#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, 1-_s); +//#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +//#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, 1-_s); /* for R12, this signal is active high */ -//#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -//#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, _s); +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, _s); #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) From 5d588d98be0e44b642b54efab5f6a7b8d85e406e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 13 Jan 2016 10:37:17 -0700 Subject: [PATCH 090/105] change CMake CONFIG to fmu-v2 and THREADS to 4 --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dcb33b67e6..9bd26e839c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,10 +132,10 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) set(CMAKE_BUILD_TYPE "" CACHE STRING "build type") set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ";Debug;Release;RelWithDebInfo;MinSizeRel") -set(CONFIG "nuttx_px4fmu-v4_default" CACHE STRING "desired configuration") +set(CONFIG "nuttx_px4fmu-v2_default" CACHE STRING "desired configuration") file(GLOB_RECURSE configs RELATIVE cmake/configs "cmake/configs/*.cmake") set_property(CACHE CONFIG PROPERTY STRINGS ${configs}) -set(THREADS "8" CACHE STRING +set(THREADS "4" CACHE STRING "number of threads to use for external build processes") set(DEBUG_PORT "/dev/ttyACM0" CACHE STRING "debugging port") From 4d7fe0806970878d2b2d210b7afc0a93780b36a9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 13 Jan 2016 17:15:11 -0700 Subject: [PATCH 091/105] fix code style --- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index f70f5e3725..37852242fb 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -301,12 +301,12 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 12MHz and de-assert the known chip selects. - * MS5611 has max SPI clock speed of 20MHz - */ + /* Default SPI2 to 12MHz and de-assert the known chip selects. + * MS5611 has max SPI clock speed of 20MHz + */ - // XXX start with 10.4 MHz and go up to 20 once validated - SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + // XXX start with 10.4 MHz and go up to 20 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); From ab1bbb9ed88b231fa801938bc53d7d5554ac9ced Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 13 Jan 2016 21:59:57 -0700 Subject: [PATCH 092/105] remove extraneous rescan tests --- src/drivers/px4fmu/fmu.cpp | 28 +++++++++++----------------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a629bb54f..03bbb688a7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -136,7 +136,6 @@ private: "ST24" }; - hrt_abstime _rc_scan_last_lock = 0; hrt_abstime _rc_scan_begin = 0; bool _rc_scan_locked = false; bool _report_lock = true; @@ -930,7 +929,7 @@ PX4FMU::cycle() sbus_config(_rcs_fd, false); rc_io_invert(true); - } else if (now - _rc_scan_last_lock < rc_scan_max + } else if (_rc_scan_locked || now - _rc_scan_begin < rc_scan_max) { // parse new data @@ -942,12 +941,11 @@ PX4FMU::cycle() // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_last_lock = now; - _rc_scan_locked = true; + _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_DSM); } @@ -961,7 +959,7 @@ PX4FMU::cycle() dsm_config(_rcs_fd); rc_io_invert(false); - } else if (now - _rc_scan_last_lock < rc_scan_max + } else if (_rc_scan_locked || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { @@ -973,12 +971,11 @@ PX4FMU::cycle() // we have a new DSM frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops); - _rc_scan_last_lock = now; _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_ST24); } @@ -992,7 +989,7 @@ PX4FMU::cycle() dsm_config(_rcs_fd); rc_io_invert(false); - } else if (now - _rc_scan_last_lock < rc_scan_max + } else if (_rc_scan_locked || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { @@ -1012,12 +1009,11 @@ PX4FMU::cycle() // we have a new ST24 frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, st24_rssi); - _rc_scan_last_lock = now; _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_SUMD); } @@ -1031,7 +1027,7 @@ PX4FMU::cycle() dsm_config(_rcs_fd); rc_io_invert(false); - } else if (now - _rc_scan_last_lock < rc_scan_max + } else if (_rc_scan_locked || now - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { @@ -1051,12 +1047,11 @@ PX4FMU::cycle() // we have a new SUMD frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, sumd_rssi); - _rc_scan_last_lock = now; _rc_scan_locked = true; } } - } else if (!_rc_scan_locked) { + } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_SUMD); } @@ -1073,7 +1068,7 @@ PX4FMU::cycle() stm32_configgpio(GPIO_PPM_IN); rc_io_invert(false); - } else if (now - _rc_scan_last_lock < rc_scan_max + } else if (_rc_scan_locked || now - _rc_scan_begin < rc_scan_max) { // see if we have new PPM input data @@ -1083,11 +1078,10 @@ PX4FMU::cycle() rc_updated = true; fill_rc_in(ppm_decoded_channels, ppm_buffer, now, false, false, 0); - _rc_scan_last_lock = now; _rc_scan_locked = true; } - } else if (!_rc_scan_locked) { + } else { // disable CPPM input by mapping it away from the timer capture input stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); // Scan the next protocol From 5a93e68918466adafda0a58b1b70b0da966aedef Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 13 Jan 2016 22:01:11 -0700 Subject: [PATCH 093/105] fix code style --- src/drivers/px4fmu/fmu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 03bbb688a7..2edf117f2f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -941,7 +941,7 @@ PX4FMU::cycle() // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_locked = true; + _rc_scan_locked = true; } } @@ -1081,7 +1081,7 @@ PX4FMU::cycle() _rc_scan_locked = true; } - } else { + } else { // disable CPPM input by mapping it away from the timer capture input stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); // Scan the next protocol From c7767dfe7e9403823dd5d433dfd83c8d61d8ef59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 17:51:59 +0100 Subject: [PATCH 094/105] RC: Add constants for FMU input --- msg/input_rc.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 3e226966e6..331551f951 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -6,6 +6,9 @@ uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 uint8 RC_INPUT_SOURCE_MAVLINK = 6 uint8 RC_INPUT_SOURCE_QURT = 7 +uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 +uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 +uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. From 5bd4495a789972e79068ed4c2947bb9af4bfcf7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 17:53:02 +0100 Subject: [PATCH 095/105] Added input_rc SUMD defines --- msg/input_rc.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 331551f951..f28f14d8e7 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -9,6 +9,8 @@ uint8 RC_INPUT_SOURCE_QURT = 7 uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 +uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 +uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 12 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. From 9fb29d3a38a468bbc67850eb6a2648d963733002 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 17:55:19 +0100 Subject: [PATCH 096/105] FMU: Add RC input definition for each RC protocol --- src/drivers/px4fmu/fmu.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2edf117f2f..e017a2388d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -939,6 +939,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new SBUS frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; fill_rc_in(raw_rc_count, raw_rc_values, now, sbus_frame_drop, sbus_failsafe, frame_drops); _rc_scan_locked = true; @@ -969,6 +970,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new DSM frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops); _rc_scan_locked = true; @@ -1007,6 +1009,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new ST24 frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, st24_rssi); _rc_scan_locked = true; @@ -1045,6 +1048,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new SUMD frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; fill_rc_in(raw_rc_count, raw_rc_values, now, false, false, frame_drops, sumd_rssi); _rc_scan_locked = true; @@ -1076,6 +1080,7 @@ PX4FMU::cycle() && ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. rc_updated = true; + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; fill_rc_in(ppm_decoded_channels, ppm_buffer, now, false, false, 0); _rc_scan_locked = true; From b26fc1f089ebef751fd1aa82ffcf2a14ed6b3828 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 11 Jan 2016 11:20:43 +0100 Subject: [PATCH 097/105] fix waypoint handling in position control --- .../fw_pos_control_l1_main.cpp | 39 +++++++++++++------ 1 file changed, 28 insertions(+), 11 deletions(-) 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 d7aebbe0a8..034c48a08b 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 @@ -973,24 +973,41 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa { waypoint_prev.valid = true; waypoint_prev.alt = _hold_alt; + position_setpoint_s temp_next {}; + position_setpoint_s temp_prev {}; if (flag_init) { - // on init set first waypoint to momentary position - waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; - waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + // on init set previous waypoint HDG_HOLD_SET_BACK_DIST meters behind us + waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading + 180.0f * M_DEG_TO_RAD_F , + HDG_HOLD_SET_BACK_DIST, + &temp_prev.lat, &temp_prev.lon); + + // set next waypoint HDG_HOLD_DIST_NEXT meters in front of us + waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading, HDG_HOLD_DIST_NEXT, + &temp_next.lat, &temp_next.lon); + waypoint_prev = temp_prev; + waypoint_next = temp_next; + waypoint_next.valid = true; + waypoint_next.alt = _hold_alt; + + return; + } else { - /* - for previous waypoint use the one still in front of us but shift it such that it is - located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us - */ - waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; - waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + // for previous waypoint use the one still in front of us but shift it such that it is + // located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us + create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, + HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST, + &temp_prev.lat, &temp_prev.lon); } waypoint_next.valid = true; - waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; - waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + + create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, + -(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST), + &temp_next.lat, &temp_next.lon); + waypoint_prev = temp_prev; + waypoint_next = temp_next; waypoint_next.alt = _hold_alt; } From c0bc72177861bffcdb5bb6b22680f78f05b8c413 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 18:48:42 +0100 Subject: [PATCH 098/105] Add missing define for DSM --- msg/input_rc.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/input_rc.msg b/msg/input_rc.msg index f28f14d8e7..0896bdd34f 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -10,7 +10,8 @@ uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 -uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 12 +uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 +uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. From c0e88e262c8d01d22e5b0df00ab95a8a1576de86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Jan 2016 23:50:37 +0100 Subject: [PATCH 099/105] Drop man min throttle to 8% since its a continous user complaint --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 3e00c5f58c..80792304d9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); /** * Maximum manual thrust From c77a2acb93e62f34649a44161e9917423867e472 Mon Sep 17 00:00:00 2001 From: Sander Smeets Date: Fri, 15 Jan 2016 10:22:29 +0000 Subject: [PATCH 100/105] QuadRanger airframe --- ROMFS/px4fmu_common/init.d/13008_QuadRanger | 53 +++++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13008_QuadRanger diff --git a/ROMFS/px4fmu_common/init.d/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/13008_QuadRanger new file mode 100644 index 0000000000..16b701c596 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13008_QuadRanger @@ -0,0 +1,53 @@ +#!nsh +# +# @name QuadRanger +# +# @type Standard VTOL +# +# @maintainer Sander Smeets +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set PWM_AUX_REV1 1 + param set PWM_AUX_REV2 1 + + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 From be1db2ced51d837dc062aa9d509955c0aa41bfab Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 15 Jan 2016 15:52:12 +0100 Subject: [PATCH 101/105] quick fix: Remove throttle non-increase condition for landing since this has lead to quads falling out of the sky. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 a604b4c263..3bc8f9797e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1494,6 +1494,9 @@ MulticopterPositionControl::task_main() _landing_started = hrt_absolute_time(); } + #if 0 + // TODO quick fix: remove this since in combination with the non-working fall detection + // it can lead to the copter falling out of the sky /* don't let it throttle up again during landing */ if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust /* fix landing thrust after a certain time when velocity change is minimal */ @@ -1502,6 +1505,7 @@ MulticopterPositionControl::task_main() && hrt_elapsed_time(&_landing_started) > 15e5) { _landing_thrust = thrust_abs; } + #endif /* assume ground, reduce thrust */ if (hrt_elapsed_time(&_landing_started) > 15e5 From e663b60c694b08306b8a1257faf6ec5edd39ec38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Jan 2016 22:34:09 +0100 Subject: [PATCH 102/105] FMUv2: Add baro SPI bus define --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 12593937f7..87fce378a9 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -115,6 +115,7 @@ __BEGIN_DECLS #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_EXT 4 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 From 0910cb32561595a78b647674227938912a31272a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Jan 2016 22:34:20 +0100 Subject: [PATCH 103/105] FMUv4: Add baro SPI bus define --- src/drivers/boards/px4fmu-v4/board_config.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index a2a6562aa7..c4a9763d65 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -120,6 +120,7 @@ __BEGIN_DECLS #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_RAMTRON 2 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_RAMTRON /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 @@ -133,6 +134,9 @@ __BEGIN_DECLS * use 3 for the barometer to differentiate */ #define PX4_SPIDEV_BARO 3 +#if (PX4_SPIDEV_BARO == SPIDEV_FLASH) + #error PX4_SPIDEV_BARO must not be equal to SPIDEV_FLASH as they share the same bus +#endif /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 From 415e42f5de5b52c411a260aa9911f28bee91c624 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Jan 2016 22:44:22 +0100 Subject: [PATCH 104/105] MS5611: Use baro SPI device --- src/drivers/ms5611/ms5611_nuttx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index bc4ac4768d..c2e7c11361 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -829,7 +829,7 @@ struct ms5611_bus_option { { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, #endif #ifdef PX4_SPIDEV_BARO - { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_RAMTRON, NULL }, + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_BARO, NULL }, #endif #ifdef PX4_I2C_BUS_ONBOARD { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, From 7ea41491e540426a37e9e2d8759e660a25bd101e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Jan 2016 23:45:29 +0100 Subject: [PATCH 105/105] FMUv4: Fix code style --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index c4a9763d65..062f817998 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -135,7 +135,7 @@ __BEGIN_DECLS */ #define PX4_SPIDEV_BARO 3 #if (PX4_SPIDEV_BARO == SPIDEV_FLASH) - #error PX4_SPIDEV_BARO must not be equal to SPIDEV_FLASH as they share the same bus +#error PX4_SPIDEV_BARO must not be equal to SPIDEV_FLASH as they share the same bus #endif /* I2C busses */