semaphore: add px4_sem_trywait

directly mapped to the posix method sem_trywait

src/platforms/posix/px4_layer/px4_sem.cpp

@@ -91,6 +91,27 @@ int px4_sem_wait(px4_sem_t *s)
 	return (ret) ? ret : mret;
 }
 
+int px4_sem_trywait(px4_sem_t *s)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		return ret;
+	}
+
+	if (s->value <= 0) {
+		errno = EAGAIN;
+		ret = -1;
+
+	} else {
+		s->value--;
+	}
+
+	int mret = pthread_mutex_unlock(&(s->lock));
+
+	return (ret) ? ret : mret;
+}
+
 int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime)
 {
 	int ret = pthread_mutex_lock(&(s->lock));

src/platforms/px4_sem.h

@@ -63,6 +63,7 @@ typedef struct {
 __EXPORT int		px4_sem_init(px4_sem_t *s, int pshared, unsigned value);
 __EXPORT int		px4_sem_setprotocol(px4_sem_t *s, int protocol);
 __EXPORT int		px4_sem_wait(px4_sem_t *s);
+__EXPORT int		px4_sem_trywait(px4_sem_t *sem);
 __EXPORT int		px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime);
 __EXPORT int		px4_sem_post(px4_sem_t *s);
 __EXPORT int		px4_sem_getvalue(px4_sem_t *s, int *sval);
@@ -79,6 +80,7 @@ typedef sem_t px4_sem_t;
 #define px4_sem_init		sem_init
 #define px4_sem_setprotocol sem_setprotocol
 #define px4_sem_wait		sem_wait
+#define px4_sem_trywait	sem_trywait
 #define px4_sem_post		sem_post
 #define px4_sem_getvalue	sem_getvalue
 #define px4_sem_destroy		sem_destroy