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));