From 3a474347491236fcb32ac6b58fe7d0cf78564f3b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 25 Aug 2015 21:59:01 -0700 Subject: [PATCH] Fixes for qurt HIL build Workaround required Eigen downgrade to 3.2. Hexagon toolchain does not support C++11 features of newest version of Eigen. Running make qurt_fixup will downgrade and patch Eigen for qurt. Running make restore will revert the patch and do a git submodule update to restore the expected Eigen version. Added a "restore" target to undo qurt_fixup Before doing a qurt build run: make qurt_fixup That will downgrade Eigen to 3.2 and apply the require patch. To build another target after downgrading: make restore Them make the desired target (other than qurt). Fixed type used in orb_priority to be consistent with the code (int* was used in declaration but int32_t* used in code) Removed unused class member variable in sensors.cpp Added cmake fix for unit tests. The location of px4_log.c changed. Fixed the qurt drv_hrt.c implementation to use us instead of ms for time resolution Added px4_led.c to nuttx platform layer Use the posix version of px4_led.c for nuttx so we don't end up with duplicate files. It was moved out of common because it is not used by qurt. Changed PX4_DEBUG to PX4_WARN when checking for the error condition for store_poll_waiter in vdev.cpp Updated the px4_log.h file to make calls to the qurt_log functions. The qurt_log function is defined in the platforms/qurt layer. Added an option to control starting the commander module in HIL mode. Moved the flight specific drivers to the configuration file instead of adding them to the common tool chain file because HIL mode does not need them. Added the uorb Subscriber and Publisher classes Call PX4_ISFINITE macro instead of isfinite(). Added px4_led.c to nuttx platform layer Use the posix version of px4_led.c for nuttx so we don't end up with duplicate files. It was moved out of common because it is not used by qurt. Signed-off-by: Mark Charlebois --- Makefile | 9 +- Tools/check_submodules.sh | 31 +-- makefiles/qurt/config_qurt_adsp.mk | 6 + makefiles/qurt/config_qurt_hil.mk | 1 + makefiles/qurt/setup.sh | 48 +++++ makefiles/qurt/toolchain_hexagon.mk | 4 - readme.md | 0 src/drivers/device/vdev.cpp | 5 +- src/drivers/device/vdev.h | 14 +- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 2 +- src/modules/commander/commander.cpp | 18 +- .../position_estimator_inav_main.c | 6 +- src/modules/sensors/sensors.cpp | 18 +- src/modules/uORB/module.mk | 9 +- src/modules/uORB/uORB.h | 2 +- src/platforms/common/module.mk | 3 +- src/platforms/common/px4_log.c | 5 - src/platforms/nuttx/px4_layer/module.mk | 3 +- src/platforms/posix/px4_layer/module.mk | 1 + src/platforms/posix/px4_layer/px4_log.c | 5 + src/platforms/px4_log.h | 95 +++++++-- src/platforms/px4_workqueue.h | 4 + .../qurt/eagle_px4_firmware_patch.patch | 190 ++++++++++++++++++ src/platforms/qurt/include/qurt_log.h | 64 ++++++ src/platforms/qurt/px4_layer/commands_hil.c | 2 +- src/platforms/qurt/px4_layer/drv_hrt.c | 16 +- src/platforms/qurt/px4_layer/main.cpp | 8 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 4 +- src/systemcmds/mixer/mixer.cpp | 6 +- unittests/CMakeLists.txt | 2 +- 30 files changed, 484 insertions(+), 97 deletions(-) create mode 100755 makefiles/qurt/setup.sh create mode 100644 readme.md delete mode 100644 src/platforms/common/px4_log.c create mode 100644 src/platforms/posix/px4_layer/px4_log.c create mode 100644 src/platforms/qurt/eagle_px4_firmware_patch.patch create mode 100644 src/platforms/qurt/include/qurt_log.h diff --git a/Makefile b/Makefile index 4632699433..a0dc23f705 100644 --- a/Makefile +++ b/Makefile @@ -118,6 +118,13 @@ ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/qurt/firmware_qurt.mk endif +qurt_fixup: + makefiles/qurt/setup.sh $(PX4_BASE) + +restore: + cd src/lib/eigen && git checkout . + git submodule update + # # Versioning # @@ -155,7 +162,7 @@ size: .PHONY: checksubmodules checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh $(PX4_TARGET_OS)) .PHONY: updatesubmodules updatesubmodules: diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index c2c6e3d7bb..5fa0831438 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -73,20 +73,27 @@ fi if [ -d src/lib/eigen ] then - STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") - if [ -z "$STATUSRETVAL" ] + echo "ARG = $1" + if [ $1 = "qurt" ] then - echo "Checked Eigen submodule, correct version found" + # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 + STATUSRETVAL=$(true) else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "eigen sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 + STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked Eigen submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "eigen sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi fi else git submodule update --init --recursive diff --git a/makefiles/qurt/config_qurt_adsp.mk b/makefiles/qurt/config_qurt_adsp.mk index 75b1bc429f..6a1ce1d5f2 100644 --- a/makefiles/qurt/config_qurt_adsp.mk +++ b/makefiles/qurt/config_qurt_adsp.mk @@ -1,3 +1,9 @@ +#Added configuration specific flags here. + +# For Actual flight we need to link against the driver dynamic libraries +LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 +LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc + # # Makefile for the EAGLE QuRT *default* configuration # diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index ee0288d764..0536d7a4ad 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -57,6 +57,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += modules/controllib # # QuRT port diff --git a/makefiles/qurt/setup.sh b/makefiles/qurt/setup.sh new file mode 100755 index 0000000000..8ffb13cc4b --- /dev/null +++ b/makefiles/qurt/setup.sh @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# ########################################################################### + +# +# This file is required to modify the PX4 project so it can be build with +# the Hexagon toolchain. +# +# 1. Downgrade Eigen to 3.2 because the C++11 features of latest version +# are not supported +# 2. Patch Eigen because the Hexagon toolchain complex.h defines "I" +# + +cd $1 +cd src/lib/eigen/ +git checkout . +git checkout e7850ed81f9c469e02df496ef09ae32ec0379b71 +patch -p1 < ../../../makefiles/qurt/qurt_eigen.patch + diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 556c2e21dc..038c299904 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -246,10 +246,6 @@ LDFLAGS += -g -mv5 -mG0lib -G0 -fpic -shared \ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) -# driver dynamic libraries -LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 -LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc - # Compiler support library # LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) diff --git a/readme.md b/readme.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0ed4d39ada..507cebbf83 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) /* yes? post the notification */ if (fds->revents != 0) sem_post(fds->sem); + } else { + PX4_WARN("Store Poll Waiter error."); } } else { @@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device - +} // namespace device \ No newline at end of file diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 3fd28c82ca..c39857bbf0 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -324,6 +324,13 @@ public: static const char *devList(unsigned int *next); static const char *topicList(unsigned int *next); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + protected: int register_driver(const char *name, void *data); @@ -406,13 +413,6 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - bool _pub_blocked; /**< true if publishing should be blocked */ private: diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 4bff7c1123..f42d166535 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -406,7 +406,7 @@ PWMSim::task_main() ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ - if (_mixers != nullptr) { + if (_armed && _mixers != nullptr) { /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 847c74f06e..a3996bf9e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -877,6 +877,17 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_armed = false; + bool startup_in_hil = false; + if(argc>0) { + if ( strcmp(argv[0],"-hil")==0) { + startup_in_hil = true; + } else { + PX4_ERR("Argument %s not supported.",argv[0]); + PX4_ERR("COMMANDER NOT STARTED"); + thread_should_exit = true; + } + } + /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); @@ -958,7 +969,12 @@ int commander_thread_main(int argc, char *argv[]) status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - status.hil_state = vehicle_status_s::HIL_STATE_OFF; + + if(startup_in_hil) { + status.hil_state = vehicle_status_s::HIL_STATE_ON; + } else { + status.hil_state = vehicle_status_s::HIL_STATE_OFF; + } status.failsafe = false; /* neither manual nor offboard control commands have been received */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 05687d0943..892e50f3a1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,10 +116,10 @@ static inline int max(int val1, int val2) static void usage(const char *reason) { if (reason) { - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); } - fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n"); return; } @@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (isfinite(sensor.baro_alt_meter)) { + if (PX4_ISFINITE(sensor.baro_alt_meter)) { baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cd5db542d3..b9337dc231 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -239,7 +239,6 @@ private: int32_t _mag2_prio; /**< mag2 sensor priority */ int32_t _baro_prio; /**< baro0 sensor priority */ int32_t _baro1_prio; /**< baro1 sensor priority */ - int32_t _diff_pres_prio; /**< baro1 sensor priority */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -1526,6 +1525,12 @@ Sensors::parameter_update_poll(bool forced) /* run through all mag sensors */ for (unsigned s = 0; s < 3; s++) { + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); @@ -1536,12 +1541,6 @@ Sensors::parameter_update_poll(bool forced) continue; } - /* set a valid default rotation (same as board). - * if the mag is configured, this might be replaced - * in the section below. - */ - _mag_rotation[s] = _board_rotation; - bool config_ok = false; /* run through all stored calibrations */ @@ -2124,7 +2123,7 @@ Sensors::task_main() warnx("Sensor initialization failed"); _sensors_task = -1; if (_fd_adc >=0) { - close(_fd_adc); + px4_close(_fd_adc); _fd_adc = -1; } return; @@ -2169,9 +2168,6 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */ diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 6a71fc25f4..b483c00a80 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm) SRCS += uORBTest_UnitTest.cpp endif -ifneq ($(PX4_TARGET_OS),qurt) -SRCS += Publication.cpp \ - Subscription.cpp -endif - SRCS += objects_common.cpp \ uORBUtils.cpp \ uORB.cpp \ - uORBMain.cpp + uORBMain.cpp \ + Publication.cpp \ + Subscription.cpp MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 3ae0f56a30..7b40cff219 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -338,7 +338,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_priority(int handle, int *priority) __EXPORT; +extern int orb_priority(int handle, int32_t *priority) __EXPORT; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk index 472dc7dba6..0e5f463e69 100644 --- a/src/platforms/common/module.mk +++ b/src/platforms/common/module.mk @@ -2,6 +2,5 @@ # Common OS porting APIs # -SRCS = px4_getopt.c \ - px4_log.c +SRCS = px4_getopt.c diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c deleted file mode 100644 index a2c61ab297..0000000000 --- a/src/platforms/common/px4_log.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; - -__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk index 1738063a56..93b55e67ab 100644 --- a/src/platforms/nuttx/px4_layer/module.mk +++ b/src/platforms/nuttx/px4_layer/module.mk @@ -35,6 +35,7 @@ # NuttX porting layer # -SRCS = px4_nuttx_tasks.c +SRCS = px4_nuttx_tasks.c \ + ../../posix/px4_layer/px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 7d7e75afee..846ce9469c 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -40,5 +40,6 @@ SRCS = \ px4_posix_tasks.cpp \ lib_crc32.c \ drv_hrt.c \ + px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c new file mode 100644 index 0000000000..238b324194 --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -0,0 +1,5 @@ +#include + +__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; + +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index ab6e0f287c..d071893b0c 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -38,6 +38,24 @@ #pragma once +#define _PX4_LOG_LEVEL_ALWAYS 0 +#define _PX4_LOG_LEVEL_DEBUG 1 +#define _PX4_LOG_LEVEL_WARN 2 +#define _PX4_LOG_LEVEL_ERROR 3 +#define _PX4_LOG_LEVEL_PANIC 4 + +// Used to silence unused variable warning +static inline void do_nothing(int level, ...) +{ + (void)level; +} + +/**************************************************************************** + * __px4_log_omit: + * Compile out the message + ****************************************************************************/ +#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) + #if defined(__PX4_ROS) #include @@ -47,6 +65,54 @@ #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) #define PX4_DEBUG(...) +#elif defined(__PX4_QURT) +#include "qurt_log.h" +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#endif +#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) + #else #define __STDC_FORMAT_MACROS @@ -59,18 +125,6 @@ __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); -// Used to silence unused variable warning -static inline void do_nothing(int level, ...) -{ - (void)level; -} - -#define _PX4_LOG_LEVEL_ALWAYS 0 -#define _PX4_LOG_LEVEL_PANIC 1 -#define _PX4_LOG_LEVEL_ERROR 2 -#define _PX4_LOG_LEVEL_WARN 3 -#define _PX4_LOG_LEVEL_DEBUG 4 - __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; @@ -107,11 +161,22 @@ __EXPORT extern int __px4_log_level_current; * Use these to implement the code level macros below ****************************************************************************/ + /**************************************************************************** - * __px4_log_omit: - * Compile out the message + * __px4_log_named_cond: + * Convert a message in the form: + * PX4_LOG_COND(__dbg_enabled, "val is %d", val); + * to + * printf("%-5s val is %d\n", "LOG", val); + * if the first arg/condition is true. ****************************************************************************/ -#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) +#define __px4_log_named_cond(name, cond, FMT, ...) \ + __px4__log_startcond(cond)\ + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + __px4__log_endline /**************************************************************************** * __px4_log_named_cond: diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 79162e3f6b..76e1218ba5 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -47,6 +47,10 @@ #include #include +#ifdef __PX4_QURT + #include +#endif + __BEGIN_DECLS #define HPWORK 0 diff --git a/src/platforms/qurt/eagle_px4_firmware_patch.patch b/src/platforms/qurt/eagle_px4_firmware_patch.patch new file mode 100644 index 0000000000..41ac284b51 --- /dev/null +++ b/src/platforms/qurt/eagle_px4_firmware_patch.patch @@ -0,0 +1,190 @@ +diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h +index 4ba2f98..a7b664e 100644 +--- a/src/include/mavlink/mavlink_log.h ++++ b/src/include/mavlink/mavlink_log.h +@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink critical message and print to console. +@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__) ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink emergency message and print to console. +@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); +- ++*/ + struct mavlink_logmessage { + char text[MAVLINK_LOG_MAXLEN + 1]; + unsigned char severity; +diff --git a/src/lib/eigen b/src/lib/eigen +--- a/src/lib/eigen ++++ b/src/lib/eigen +@@ -1 +1 @@ +-Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 ++Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty +diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp +index bbf5f8e..50c7d8b 100644 +--- a/src/modules/commander/PreflightCheck.cpp ++++ b/src/modules/commander/PreflightCheck.cpp +@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro + } + } + ++ ++#ifdef __PX4_QURT ++ // WARNING: Preflight checks are important and should be added back when ++ // all the sensors are supported ++ PX4_WARN("WARNING: Preflight checks PASS always."); ++ failed = false; ++#endif ++ + /* Report status */ + return !failed; + } +diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp +index 971e086..c165322 100644 +--- a/src/modules/commander/commander.cpp ++++ b/src/modules/commander/commander.cpp +@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[]) + } + + /* RC input check */ ++#ifndef __PX4_QURT + if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && +- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++#else ++ // HACK: remove old data check due to timestamp issue in QURT ++ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) { ++#endif + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; +@@ -2526,7 +2531,7 @@ set_control_mode() + /* set vehicle_control_mode according to set_navigation_state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); +- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; ++ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; + control_mode.flag_control_offboard_enabled = false; + + switch (status.nav_state) { +diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c +index 892e50f..7ac6776 100644 +--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c ++++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c +@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) + }; + + /* wait for initial baro value */ ++#ifdef __PX4_QURT ++ // WARNING skipping ++ PX4_WARN("Hacked to skip baro initialization for testing."); ++ bool wait_baro = false; ++#else + bool wait_baro = true; ++#endif + + thread_running = true; + +diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp +index 3fa4458..0ef1883 100644 +--- a/src/modules/sensors/sensors.cpp ++++ b/src/modules/sensors/sensors.cpp +@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + ++#ifdef __PX4_QURT ++// Temporary fix required due to lack of time sync between apps and dsp cores ++ raw.timestamp = hrt_absolute_time(); ++#else + raw.timestamp = gyro_report.timestamp; ++#endif + raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; + } +@@ -2068,6 +2073,7 @@ Sensors::task_main() + + /* start individual sensors */ + int ret = 0; ++#if 0 + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; +@@ -2091,7 +2097,7 @@ Sensors::task_main() + } + return; + } +- ++#endif + /* + * do subscriptions + */ +diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp +index 83a9378..ec86faf 100644 +--- a/src/systemcmds/mixer/mixer.cpp ++++ b/src/systemcmds/mixer/mixer.cpp +@@ -118,6 +118,8 @@ load(const char *devname, const char *fname) + return 1; + } + ++#ifndef __PX4_QURT ++ + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { + warnx("can't load mixer: %s", fname); + return 1; +@@ -125,6 +127,24 @@ load(const char *devname, const char *fname) + + /* XXX pass the buffer to the device */ + int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); ++#else ++ char newbuf[] = ++ "R: 4x 10000 10000 10000 0\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 4 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 5 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 6 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 7 10000 10000 0 -10000 10000\n"; ++ /* XXX pass the buffer to the device */ ++ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); ++#endif + + if (ret < 0) { + warnx("error loading mixers from %s", fname); diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h new file mode 100644 index 0000000000..29ef6302e1 --- /dev/null +++ b/src/platforms/qurt/include/qurt_log.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef QURT_LOG_H +#define QURT_LOG_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//void qurt_log(int level, const char *file, int line, const char *format, ...); + +// declaration to make the compiler happy. This symbol is part of the adsp static image. +void HAP_debug(const char *msg, int level, const char *filename, int line); + +static __inline void qurt_log(int level, const char *file, int line, + const char *format, ...) +{ + char buf[256]; + va_list args; + va_start(args, format); + vsnprintf(buf, sizeof(buf), format, args); + va_end(args); + HAP_debug(buf, level, file, line); +} + +#ifdef __cplusplus +} +#endif + +#endif // QURT_LOG_H + diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index b795fba341..3f6c23b732 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -48,7 +48,7 @@ const char *get_commands() "param set CAL_MAG0_ID 196608\n" // "rgbled start\n" // "tone_alarm start\n" - "commander start\n" + "commander start -hil\n" "sensors start\n" //"ekf_att_pos_estimator start\n" "attitude_estimator_q start\n" diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 916992cae4..45be19ec8a 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -46,8 +46,6 @@ static struct sq_queue_s callout_queue; -extern uint64_t get_abs_time_in_us(); - /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; @@ -83,15 +81,9 @@ static void hrt_unlock(void) */ hrt_abstime hrt_absolute_time(void) { - - return get_abs_time_in_us(); -/* struct timespec ts; - - // FIXME - clock_gettime unsupported in QuRT - //clock_gettime(CLOCK_MONOTONIC, &ts); + clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); -*/ } /* @@ -252,7 +244,7 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); @@ -273,11 +265,11 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + ticks = USEC2TICK((next->deadline - now)); } } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index da25cabcd2..f4a538a13a 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -165,11 +165,9 @@ int dspal_entry( int argc, char* argv[] ) px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for( ;; ) - { - volatile int x = 0; - ++x; - } + for( ;; ){ + usleep( 1000000 ); + } return 0; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index fcae64f8c5..9925e05dfa 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -59,9 +59,7 @@ __BEGIN_DECLS extern uint64_t get_ticks_per_us(); -// FIXME - sysconf(_SC_CLK_TCK) not supported -//long PX4_TICKS_PER_SEC = get_ticks_per_us(); -long PX4_TICKS_PER_SEC = 800000000; +long PX4_TICKS_PER_SEC = 1000; unsigned int sleep(unsigned int sec) { diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 8889f4531f..83a9378619 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -91,10 +91,10 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); - fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer load \n"); + PX4_INFO("usage:\n"); + PX4_INFO(" mixer load \n"); } static int diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 99a981f521..80eb8c24e6 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -75,7 +75,7 @@ endfunction() add_library( px4_platform # ${PX_SRC}/platforms/common/px4_getopt.c - ${PX_SRC}/platforms/common/px4_log.c + ${PX_SRC}/platforms/posix/px4_layer/px4_log.c ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp ${PX_SRC}/platforms/posix/work_queue/work_lock.c