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