Fixes for qurt build

Added missing functions that were added for other targets but not for qurt.

Added workaround for missing sem_timedwait(). This may have a performance
impact until a sem_timedwait is supported.

std::to_string is not supported by the hexagon compiler

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/lib/matrix

@@ -1 +1 @@
-Subproject commit cc1658db15d720e05d577787363823be0f2bd161
+Subproject commit b9d6cac199bd4476c6268f7eba76c01bf29f0295

src/modules/commander/commander.cpp

@@ -3209,7 +3209,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
 void *commander_low_prio_loop(void *arg)
 {
 	/* Set thread name */
-	px4_prctl(PR_SET_NAME, "commander_low_prio", getpid());
+	px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));

src/modules/systemlib/rc_check.h

@@ -36,6 +36,7 @@
  *
  * RC calibration check
  */
+#include <stdbool.h>
 
 #pragma once
 

src/platforms/posix/px4_layer/drv_hrt.c

@@ -81,7 +81,7 @@ static void hrt_unlock(void)
 	px4_sem_post(&_hrt_lock);
 }
 
-#if defined(__APPLE__) && defined(__MACH__)
+#if (defined(__APPLE__) && defined(__MACH__)) || defined(__PX4_QURT)
 #include <time.h>
 #include <sys/time.h>
 #define CLOCK_REALTIME 0

src/platforms/px4_defines.h

@@ -177,8 +177,7 @@ __END_DECLS
 #define MAX_RAND 32767
 
 #if defined(__PX4_QURT)
-#define M_PI			3.14159265358979323846
-#define M_PI_2			1.57079632679489661923
+#include "dspal_math.h"
 __BEGIN_DECLS
 #include <math.h>
 __END_DECLS

src/platforms/px4_posix.h

@@ -77,11 +77,16 @@ typedef sem_t px4_sem_t;
 
 #define px4_sem_init	 sem_init
 #define px4_sem_wait	 sem_wait
-#define px4_sem_timedwait	 sem_timedwait
 #define px4_sem_post	 sem_post
 #define px4_sem_getvalue sem_getvalue
 #define px4_sem_destroy	 sem_destroy
 
+#ifdef __PX4_QURT
+__EXPORT int		px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime);
+#else
+#define px4_sem_timedwait	 sem_timedwait
+#endif
+
 __END_DECLS
 
 #endif

src/platforms/qurt/include/qurt_log.h

@@ -35,11 +35,14 @@
 
 #include <stdarg.h>
 #include <stdio.h>
+#include <stdint.h>
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+__EXPORT extern uint64_t hrt_absolute_time(void);
+
 //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.

src/platforms/qurt/px4_layer/drv_hrt.c

@@ -63,8 +63,36 @@ static void		hrt_call_reschedule(void);
 static sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
 
-static void
-hrt_call_invoke(void);
+#include <time.h>
+#include <sys/time.h>
+#define CLOCK_REALTIME 0
+
+#ifndef CLOCK_MONOTONIC
+#define CLOCK_MONOTONIC 1
+#endif
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	struct timeval now;
+	int rv = gettimeofday(&now, NULL);
+
+	if (rv) {
+		return rv;
+	}
+
+	tp->tv_sec = now.tv_sec;
+	tp->tv_nsec = now.tv_usec * 1000;
+
+	return 0;
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	/* do nothing right now */
+	return 0;
+}
+
+static void hrt_call_invoke(void);
 
 static void hrt_lock(void)
 {

src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@@ -37,7 +37,10 @@
  * Implementation of existing task API for Linux
  */
 
-#include <px4_log.h>
+#include "px4_log.h"
+#include "px4_posix.h"
+#include "px4_workqueue.h"
+#include "hrt_work.h"
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -318,7 +321,7 @@ void px4_show_tasks()
 
 __BEGIN_DECLS
 
-int px4_getpid()
+unsigned long px4_getpid()
 {
 	pthread_t pid = pthread_self();
 //
@@ -333,7 +336,7 @@ int px4_getpid()
 	}
 
 	PX4_ERR("px4_getpid() called from unknown thread context!");
-	return -EINVAL;
+	return ~0;
 }
 
 
@@ -350,5 +353,44 @@ const char *getprogname()
 
 	return "Unknown App";
 }
-__END_DECLS
 
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+
+	sem_post(sem);
+}
+
+int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
+{
+	void *result;
+	work_s _hpwork = {};
+
+	// Create a timer to unblock
+	uint32_t timeout = ts->tv_sec*1000000+ (ts->tv_nsec/1000);
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
+	return 0;
+}
+
+int px4_prctl(int option, const char *arg2, unsigned pid)
+{
+	int rv;
+
+	switch (option) {
+	case PR_SET_NAME:
+		// set the threads name - Not supported
+		// rv = pthread_setname_np(pthread_self(), arg2);
+		rv = -1;
+		break;
+
+	default:
+		rv = -1;
+		PX4_WARN("FAILED SETTING TASK NAME");
+		break;
+	}
+
+	return rv;
+}
+__END_DECLS