diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 4574574999..8f44a84946 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -38,9 +38,7 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 -#HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 -HEXAGON_SDK_ROOT = /prj/atlanticus/users/rkintada/Hexagon_SDK/2.0 -#V_ARCH = v4 +HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 07fe6b35eb..f90a0a6237 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -395,12 +395,14 @@ private: float _rate_txerr; float _rate_rx; +#ifdef __PX4_POSIX int _socket_fd; struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; Protocol _protocol; unsigned short _udp_port; +#endif struct telemetry_status_s _rstatus; ///< receive status diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 037ff395b3..ddbce3ff55 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -91,7 +91,7 @@ uORB::DeviceNode::open(device::file_t *filp) lock(); if (_publisher == 0) { - _publisher = getpid(); + _publisher = px4_getpid(); ret = PX4_OK; } else { @@ -153,7 +153,7 @@ uORB::DeviceNode::close(device::file_t *filp) { //warnx("uORB::DeviceNode::close fd = %d", filp->fd); /* is this the publisher closing? */ - if (getpid() == _publisher) { + if (px4_getpid() == _publisher) { _publisher = 0; } else { diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 0461689f2a..d655fa91d6 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -52,7 +52,7 @@ } #if defined(__PX4_QURT) #include -#include "HAP_farf.h" +#define FARF printf #define __FARF_omit(level, ...) { } #define __FARF_log(level, ...) { \ @@ -66,10 +66,10 @@ FARF(" (file %s line %d)\n", __FILE__, __LINE__);\ } -#define PX4_DEBUG(...) __FARF_omit(HIGH, __VA_ARGS__) -#define PX4_INFO(...) __FARF_log(HIGH, __VA_ARGS__) -#define PX4_WARN(...) __FARF_log_verbose(HIGH, __VA_ARGS__) -#define PX4_ERR(...) __FARF_log_verbose(HIGH, __VA_ARGS__) +#define PX4_DEBUG(...) __FARF_omit("DEBUG", __VA_ARGS__) +#define PX4_INFO(...) __FARF_log("INFO", __VA_ARGS__) +#define PX4_WARN(...) __FARF_log_verbose("WARN", __VA_ARGS__) +#define PX4_ERR(...) __FARF_log_verbose("ERROR", __VA_ARGS__) #elif defined(__PX4_LINUX) #include diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 9c392ac42a..da3e584b80 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -68,6 +68,7 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_poll _GLOBAL poll #define px4_fsync _GLOBAL fsync #define px4_access _GLOBAL access +#define px4_getpid _GLOBAL getpid #elif defined(__PX4_POSIX) @@ -98,6 +99,12 @@ __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT int px4_fsync(int fd); __EXPORT int px4_access(const char *pathname, int mode); +#ifdef __PX4_QURT +typedef int pid_t; +__EXPORT int px4_getpid(void); +#else +#define px4_getpid getpid +#endif __END_DECLS #else diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 0c9b7f24c9..a54a18b9ab 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -28,7 +28,6 @@ struct timespec int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); -__EXPORT int usleep(useconds_t usec); __EXPORT unsigned int sleep(unsigned int sec); __END_DECLS diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 0987c6feb1..9087fd873e 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include "systemlib/param/param.h" @@ -57,14 +56,10 @@ __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported long PX4_TICKS_PER_SEC = 1000; -void usleep(useconds_t usec) { - qurt_timer_sleep(usec); -} - unsigned int sleep(unsigned int sec) { for (unsigned int i=0; i< sec; i++) - qurt_timer_sleep(1000000); + usleep(1000000); return 0; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index d688f1104a..75239b91d6 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -245,6 +245,20 @@ int px4_task_kill(px4_task_t id, int sig) return rv; } +pid_t px4_getpid() +{ + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (int i=0; i