From caa94d58a6c28eaff7ed38eca2167783f9e2af66 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 3 Feb 2016 11:38:57 -0800 Subject: [PATCH] Manually integrated Jim's changes from integrate2_jim branch Signed-off-by: Jim Wilson Signed-off-by: Mark Charlebois --- cmake/configs/posix_eagle_release.cmake | 1 + .../Toolchain-arm-linux-gnueabihf.cmake | 17 ++++- src/firmware/qurt/CMakeLists.txt | 1 + .../muorb/krait/uORBKraitFastRpcChannel.cpp | 2 +- .../muorb/krait/uORBKraitFastRpcChannel.hpp | 10 ++- src/modules/systemlib/param/param_shmem.c | 67 +++++++++++++------ src/platforms/posix/px4_layer/drv_hrt.c | 14 ++-- src/platforms/px4_posix.h | 6 +- src/platforms/px4_tasks.h | 4 +- src/platforms/qurt/dspal/dspal_stub.c | 15 +---- src/platforms/qurt/px4_layer/main.cpp | 1 + .../qurt/px4_layer/px4_qurt_tasks.cpp | 25 ++++--- src/platforms/qurt/px4_layer/shmem_qurt.c | 22 +++--- src/systemcmds/mixer/mixer.cpp | 6 +- 14 files changed, 117 insertions(+), 74 deletions(-) diff --git a/cmake/configs/posix_eagle_release.cmake b/cmake/configs/posix_eagle_release.cmake index e8cc2141fd..fbf744aa48 100644 --- a/cmake/configs/posix_eagle_release.cmake +++ b/cmake/configs/posix_eagle_release.cmake @@ -5,6 +5,7 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu set(CONFIG_SHMEM "1") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_SHMEM") + include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake) set(config_module_list diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake index 02464883ca..3e331fe2fa 100644 --- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake +++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake @@ -36,13 +36,21 @@ set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_VERSION 1) # specify the cross compiler -find_program(C_COMPILER arm-linux-gnueabihf-gcc) +find_program(C_COMPILER arm-linux-gnueabihf-gcc + PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin + NO_DEFAULT_PATH + ) + if(NOT C_COMPILER) message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler") endif() cmake_force_c_compiler(${C_COMPILER} GNU) -find_program(CXX_COMPILER arm-linux-gnueabihf-g++) +find_program(CXX_COMPILER arm-linux-gnueabihf-g++ + PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin + NO_DEFAULT_PATH + ) + if(NOT CXX_COMPILER) message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler") endif() @@ -51,7 +59,10 @@ cmake_force_cxx_compiler(${CXX_COMPILER} GNU) # compiler tools foreach(tool objcopy nm ld) string(TOUPPER ${tool} TOOL) - find_program(${TOOL} arm-linux-gnueabihf-${tool}) + find_program(${TOOL} arm-linux-gnueabihf-${tool} + PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin + NO_DEFAULT_PATH + ) if(NOT ${TOOL}) message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}") endif() diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index a23a665957..203cdfc4f2 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -13,6 +13,7 @@ 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}/src/firmware/qurt/px4muorb_skel.c ${CMAKE_BINARY_DIR}/apps.h) else() diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 4fbb3727f5..534bf3bc1e 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -40,7 +40,7 @@ #define LOG_TAG "uORBKraitFastRpcChannel.cpp" -uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; +uORB::KraitFastRpcChannel *uORB::KraitFastRpcChannel::_InstancePtr = nullptr; static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics); diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp index 0e8d82b44b..d25d4d2da0 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -55,8 +55,12 @@ public: */ static uORB::KraitFastRpcChannel *GetInstance() { - return &(_Instance); - } + if (_InstancePtr == nullptr) + { + _InstancePtr = new uORB::KraitFastRpcChannel(); + } + return _InstancePtr; + } /** * @brief Interface to notify the remote entity of interest of a @@ -119,7 +123,7 @@ public: void Stop(); private: // data members - static uORB::KraitFastRpcChannel _Instance; + static uORB::KraitFastRpcChannel *_InstancePtr; uORBCommunicator::IChannelRxHandler *_RxHandler; pthread_t _RecvThread; bool _ThreadStarted; diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index fe52effbe3..86c5ce1297 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -74,7 +74,12 @@ #define debug(fmt, args...) do { } while(0) +#ifdef __PX4_QURT +//Mode not supported by qurt +#define PARAM_OPEN(a, b, ...) open(a, b) +#else #define PARAM_OPEN open +#endif #define PARAM_CLOSE close /** @@ -85,7 +90,6 @@ extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; #else -// TODO: start and end are reversed static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters; #endif @@ -517,11 +521,17 @@ param_get(param_t param, void *val) #ifdef ENABLE_SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) { PX4_INFO("param_get for %s : %d\n", param_name(param), *(int *)val); } + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO("param_get for %s : %d\n", param_name(param), ((union param_value_u *)val)->i); + } - else if (param_type(param) == PARAM_TYPE_FLOAT) { PX4_INFO("param_get for %s : %f\n", param_name(param), *(double *)val); } + else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("param_get for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f); + } - else { PX4_INFO("Unknown param type for %s\n", param_name(param)); } + else { + PX4_INFO("Unknown param type for %s\n", param_name(param)); + } #endif @@ -536,6 +546,9 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ int result = -1; bool params_changed = false; + PX4_DEBUG("param_set_internal params: param = %d, val = 0x%X, mark_saved: %d, notify_changes: %d", + param, val, (int)mark_saved, (int)notify_changes); + param_lock(); if (!handle_in_range(param)) { @@ -627,11 +640,17 @@ out: #ifdef ENABLE_SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) {PX4_INFO("param_set for %s : %d\n", param_name(param), *(int *)val);} + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO("param_set for %s : %d\n", param_name(param), ((union param_value_u *)val)->i); + } - else if (param_type(param) == PARAM_TYPE_FLOAT) {PX4_INFO("param_set for %s : %f\n", param_name(param), *(double *)val);} + else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("param_set for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f); + } - else {PX4_INFO("Unknown param type for %s\n", param_name(param));} + else { + PX4_INFO("Unknown param type for %s\n", param_name(param)); + } #endif @@ -786,34 +805,47 @@ param_get_default_file(void) int param_save_default(void) { - int res; - int fd; + int res = OK; + int fd = -1; + bool is_locked = false; const char *filename = param_get_default_file(); if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); - return 0; + res = ERROR; + goto exit; } + is_locked = true; fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd < 0) { - warn("failed to open param file: %s", filename); + PX4_ERR("failed to open param file: %s", filename); return ERROR; } res = param_export(fd, false); if (res != OK) { - warnx("failed to write parameters to file: %s", filename); + PX4_ERR("failed to write parameters to file: %s", filename); + goto exit; } PARAM_CLOSE(fd); - release_shmem_lock(); +exit: + if (is_locked) { + release_shmem_lock(); + } - PX4_INFO("saving params done\n"); + if (fd >= 0) { + close(fd); + } + + if (res == OK) { + PX4_INFO("saving params completed successfully\n"); + } return res; } @@ -855,11 +887,6 @@ param_load_default(void) static int param_load_default_no_notify(void) { - if (get_shmem_lock() != 0) { - PX4_ERR("Could not get shmem lock\n"); - return 0; - } - int fd_load = open(param_get_default_file(), O_RDONLY); if (fd_load < 0) { @@ -880,8 +907,6 @@ param_load_default_no_notify(void) PX4_INFO("param loading done\n"); - release_shmem_lock(); - if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); return -2; diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3b36dacf3c..e4e528e8cb 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -87,7 +87,7 @@ static void hrt_unlock(void) px4_sem_post(&_hrt_lock); } -#if (defined(__APPLE__) && defined(__MACH__)) || defined(__PX4_QURT) +#if (defined(__APPLE__) && defined(__MACH__)) #include #include #define CLOCK_REALTIME 0 @@ -111,12 +111,14 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) return 0; } -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) -{ - /* do nothing right now */ - return 0; -} +#elif defined(__QURT) +#include "dspal_time.h" + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + return clock_gettime(clk_id, tp); +} #endif /* diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 96db1d78cb..e0d925d0c0 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -41,12 +41,16 @@ #include #include -#include #include #include #include #include +#if defined(__PX4_QURT) +#include +#else +#include +#endif /* Semaphore handling */ diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 33c2fb4705..6b6fdbddf4 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -76,9 +76,9 @@ typedef int px4_task_t; #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT (((sched_get_priority_max(SCHED_FIFO) - sched_get_priority_min(SCHED_FIFO)) / 2) + sched_get_priority_min(SCHED_FIFO)) #elif defined(__PX4_QURT) -#define SCHED_PRIORITY_MAX 0 +#define SCHED_PRIORITY_MAX 255 #define SCHED_PRIORITY_MIN 0 -#define SCHED_PRIORITY_DEFAULT 0 +#define SCHED_PRIORITY_DEFAULT 20 #else #error "No target OS defined" #endif diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index dd97f4fc8a..72faedb719 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -34,24 +34,13 @@ #include #include -static void do_dlopen() -{ -} - -int dlinit(int a, char **b) -{ - return 0; -} - int main(int argc, char *argv[]) { int ret = 0; - char *builtin[] = {"libgcc.so", "libc.so"}; - printf("In DSPAL main\n"); - dlinit(2, builtin); + // This code is never run. It is used solely to test linking in the + // TravisCI build test - do_dlopen(); return ret; } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 5ae5ac7eab..9db1b0b0e1 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -174,6 +174,7 @@ __END_DECLS const char *get_commands() { + PX4_INFO("attempting to open the ADSP command file: %s", COMMANDS_ADSP_FILE); int fd = open(COMMANDS_ADSP_FILE, O_RDONLY); if (fd > 0) { diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 1a13558d6f..6d8c13ba58 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -112,6 +112,9 @@ px4_systemreset(bool to_bootloader) px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char *const argv[]) { + struct sched_param param; + pthread_attr_t attr; + pthread_t task; int rv; int argc = 0; int i; @@ -121,9 +124,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int char *p = (char *)argv; PX4_DEBUG("Creating %s\n", name); - pthread_t task; - pthread_attr_t attr; - struct sched_param param; + PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); // Calculate argc while (p != (char *)0) { @@ -138,7 +139,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); - pthdata_t *taskdata; + pthdata_t *taskdata = nullptr; // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize + len); @@ -164,7 +165,10 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } - rv = pthread_attr_getschedparam(&attr, ¶m); + PX4_DEBUG("stack address after pthread_attr_init: 0x%X", attr.stackaddr); + PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); + rv = pthread_attr_getschedparam(&attr, ¶m); + PX4_DEBUG("stack address after pthread_attr_getschedparam: 0x%X", attr.stackaddr); if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param"); @@ -189,28 +193,27 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int #endif size_t fixed_stacksize = -1; pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); + PX4_INFO("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); fixed_stacksize = 8 * 1024; fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; - PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + PX4_INFO("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); pthread_attr_setstacksize(&attr, fixed_stacksize); - //pthread_attr_setstacksize(&attr, stack_size); - + PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr); param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + PX4_ERR("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); if (rv != 0) { - + PX4_ERR("px4_task_spawn_cmd: pthread_create failed, error: %d", rv); return (rv < 0) ? rv : -rv; } diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index 5e644a6a88..8696f63d6a 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -47,8 +47,6 @@ #include #include -#define SHMEM_DEBUG - int mem_fd; unsigned char *map_base, *virt_addr; struct shmem_info *shmem_info_p; @@ -180,11 +178,13 @@ void update_to_shmem(param_t param, union param_value_u value) #ifdef SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) - {PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);} + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed); + } - else if (param_type(param) == PARAM_TYPE_FLOAT) - {PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, bit_changed);} + else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, bit_changed); + } #endif @@ -231,11 +231,13 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) #ifdef SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) - {PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);} + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed); + } - else if (param_type(param) == PARAM_TYPE_FLOAT) - {PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, bit_changed);} + else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, bit_changed); + } #endif } diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index c83eabc9a7..b23f57fe04 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -107,8 +107,7 @@ load(const char *devname, const char *fname) // sleep a while to ensure device has been set up usleep(20000); - int dev; - char buf[2048]; + int dev; /* open the device */ if ((dev = px4_open(devname, 0)) < 0) { @@ -122,12 +121,13 @@ load(const char *devname, const char *fname) return 1; } + char buf[2048]; if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { warnx("can't load mixer: %s", fname); return 1; } - /* XXX pass the buffer to the device */ + /* Pass the buffer to the device */ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); if (ret < 0) {