VOXL2 specific drivers, modules, and miscellaneous support files (#22588)

src/modules/muorb/apps/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 	INCLUDES
 		../test
 		../aggregator
+		libfc-sensor-api/inc
 	SRCS
 		uORBAppsProtobufChannel.cpp
 		muorb_main.cpp

src/modules/muorb/apps/fc_sensor.h

@@ -1,69 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#ifndef FC_SENSOR_H
-#define FC_SENSOR_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>
-#include <stdbool.h>
-
-typedef void (*fc_receive_cb)(const char *topic,
-			      const uint8_t *data,
-			      uint32_t length_in_bytes);
-typedef void (*fc_advertise_cb)(const char *topic);
-typedef void (*fc_add_subscription_cb)(const char *topic);
-typedef void (*fc_remove_subscription_cb)(const char *topic);
-
-typedef struct {
-	fc_receive_cb               rx_callback;
-	fc_advertise_cb             ad_callback;
-	fc_add_subscription_cb      sub_callback;
-	fc_remove_subscription_cb   remove_sub_callback;
-} fc_callbacks;
-
-int fc_sensor_initialize(bool enable_debug_messages, fc_callbacks *callbacks);
-int fc_sensor_advertise(const char *topic);
-int fc_sensor_subscribe(const char *topic);
-int fc_sensor_unsubscribe(const char *topic);
-int fc_sensor_send_data(const char *topic,
-			const uint8_t *data,
-			uint32_t length_in_bytes);
-#ifdef __cplusplus
-}
-#endif
-
-#endif // FC_SENSOR_H

src/modules/muorb/apps/uORBAppsProtobufChannel.cpp

@@ -113,6 +113,11 @@ void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic)
 		test_flag = true;
 		return;
 
+	} else if (strcmp(topic, "CPULOAD") == 0) {
+		// PX4_ERR("Got CPULOAD subscription");
+		// This will happen when a newer PX4 version is talking to a
+		// SLPI image that doesn't support the CPULOAD request. If the
+		// SLPI image does support it then we wouldn't get this.
 	} else if (_RxHandler) {
 		pthread_mutex_lock(&_rx_mutex);
 		_SlpiSubscriberCache[topic]++;

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -385,8 +385,13 @@ int px4muorb_add_subscriber(const char *topic_name)
 		uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
 
 		if (rxHandler) {
-			channel->AddRemoteSubscriber(topic_name);
-			// Pick a high message rate of 1000 Hz
+			if (channel->AddRemoteSubscriber(topic_name)) {
+				// Only process this subscription if it is the only one for the topic.
+				// Otherwise it will send some data from the queue and, most likely,
+				// mess up the queue on the remote side.
+				return 0;
+			}
+
 			return rxHandler->process_add_subscription(topic_name);
 
 		} else {
@@ -476,3 +481,32 @@ int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data,
 
 	return -1;
 }
+
+
+float px4muorb_get_cpu_load(void)
+{
+
+	// Default value to return if the SLPI code doesn't support
+	// queries for the CPU load
+	float cpu_load = 0.1f;
+
+	uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance();
+
+	if (channel) {
+		// The method to get the CPU load from the SLPI image is to send
+		// in the special code string to the add_subscription call. If it
+		// isn't supported the only return values can be 0 or -1. If it is
+		// supported then it will be some positive integer.
+		int16_t int_cpu_load = channel->add_subscription("CPULOAD", 0);
+
+		if (int_cpu_load > 1) {
+			// Yay! CPU Load query is supported!
+			cpu_load = (float) int_cpu_load;
+		}
+
+	} else {
+		PX4_ERR("Null channel pointer in %s",  __FUNCTION__);
+	}
+
+	return cpu_load;
+}

src/modules/muorb/slpi/uORBProtobufChannel.hpp

@@ -132,11 +132,13 @@ public:
 		_Aggregator.RegisterSendHandler(func);
 	}
 
-	void AddRemoteSubscriber(const std::string &messageName)
+	int AddRemoteSubscriber(const std::string &messageName)
 	{
+		int currentRemoteSubscribers;
 		pthread_mutex_lock(&_rx_mutex);
-		_AppsSubscriberCache[messageName]++;
+		currentRemoteSubscribers = _AppsSubscriberCache[messageName]++;
 		pthread_mutex_unlock(&_rx_mutex);
+		return currentRemoteSubscribers;
 	}
 
 	void RemoveRemoteSubscriber(const std::string &messageName)
@@ -214,6 +216,8 @@ extern "C" {
 	int px4muorb_remove_subscriber(const char *name) __EXPORT;
 
 	int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
+
+	float px4muorb_get_cpu_load(void) __EXPORT;
 }
 
 #endif // _uORBProtobufChannel_hpp_