uORB Explained Blog Post : Add pasta_information uORB message definition

Add details to pasta_information.msg file and add to logged_topics

Add uORBExplained Module (Not complete yet)

Add functioning uorb_explained module

- Implements a functioning module that works based on ModulBae
inheritance
- Edits the rcS script to start the module on start-up
- Implements Customer / Waiter / Chef distinction inside the code by
each function to demonstrate how uORB communication can work

Fix timestamp not logged bug + Make temperature draw sine curve

- More interesting in PlotJuggler :)

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -267,3 +267,5 @@ fi
 
 mavlink boot_complete
 replay trystart
+
+uorb_explained start

msg/CMakeLists.txt

@@ -124,6 +124,7 @@ set(msg_files
 	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
+	pasta_information.msg
 	ping.msg
 	pps_capture.msg
 	position_controller_landing_status.msg

msg/pasta_information.msg

@@ -0,0 +1,33 @@
+# Message with information for a dish of pasta
+uint64 timestamp			# [us] time when the topic was published
+
+uint16 customer_table_id		# customer’s table ID to know where to serve
+
+uint8 menu_name				# menu, e.g. Carbonara, Amatriciana
+
+uint8 PASTA_MENU_UNDEFINED = 0		# Undefined: Default value if no value is set
+uint8 PASTA_MENU_CARBONARA = 1		# Carbonara: With Egg, Pecorino and Guanciale!
+uint8 PASTA_MENU_AMATRICIANA = 2	# Amatriciana: With Tomato, Pecorino and Guanciale!
+uint8 PASTA_MENU_AGLIO_E_OLIO = 3	# Aglio E Olio: With Olive oil and Garlic!
+uint8 PASTA_MENU_BOLOGNESE = 4		# Bolognese: With Beef and Tomato!
+
+uint8 cooked_texture			# how cooked the pasta should be, e.g. Al Dente
+
+uint8 PASTA_COOKED_UNDEFINED = 0	# Undefined: Default value if no value is set
+uint8 PASTA_COOKED_AL_DENTE = 1		# Al Dente: https://en.wikipedia.org/wiki/Al_dente
+uint8 PASTA_COOKED_RAW = 2		# Barely cooked
+
+uint8 pasta_type			# type of pasta, e.g. Spaghetti, Lasagne, Rigatoni
+
+uint8 PASTA_TYPE_UNDEFINED = 0		# Undefined: Default value if no value is set
+uint8 PASTA_TYPE_SPAGHETTI = 1		# Spaghetti: Long, stringy pasta
+uint8 PASTA_TYPE_RIGATONI = 2		# Rigatoni: Cylindrical pasta perfect for Carbonara!
+uint8 PASTA_TYPE_LASAGNE = 3		# Lasagne: Flat, big pasta that gets layered on each other
+
+float32 pasta_temperature		# [deg C] temperature of the pasta
+
+# TOPICS pasta_cook pasta_order
+
+# The topic pub/sub flow would be as follows:
+# Customer -> 'pasta_order' -> Waiter
+# Waiter -> 'pasta_cook' -> Chef

src/modules/logger/logged_topics.cpp

@@ -81,6 +81,12 @@ void LoggedTopics::add_default_topics()
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");
+
+	// Pasta cook request from waiter to the chef
+	add_topic("pasta_cook");
+	// Pasta order from customer to the waiter
+	add_topic("pasta_order");
+
 	add_topic("position_controller_status", 500);
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");

src/modules/uorb_explained/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__uorb_explained
+	MAIN uorb_explained
+	STACK_MAIN 1200
+	SRCS
+		uorb_explained.cpp
+	)

src/modules/uorb_explained/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_UORB_EXPLAINED
+	bool "uorb_explained module"
+	default y
+	---help---
+		uORB Explained Module

src/modules/uorb_explained/uorb_explained.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * This module is an example module as part of the PX4 uORB Explained Series Part 4.
+ *
+ * It simulates publishing and subscribing the pasta_order and pasta_cook uORB topics!
+ */
+
+#include "uorb_explained.hpp"
+
+void uORBExplained::run()
+{
+	while (!should_exit()) {
+		hrt_abstime now = hrt_absolute_time();
+
+		// If a customer ordered a new pasta, publish (update) the new order
+		if (customer_order_pasta(now, _pasta_order_pub.get())) {
+			_pasta_order_pub.update();
+		}
+
+		// Handle Waiter relaying the order from the customer to the chef
+		waiter_handle_orders(_pasta_order_sub, _pasta_cook_pub);
+
+		// Handle Chef
+		chef_handle_orders(_pasta_cook_sub);
+
+		// Run the loop at 10 Hz, to not overload the processing power
+		px4_usleep(100_ms);
+	}
+}
+
+bool uORBExplained::customer_order_pasta(hrt_abstime now, pasta_information_s &pasta_order)
+{
+	// [us] Period of customers ordering the pasta
+	static constexpr hrt_abstime PASTA_ORDER_TIME_PERIOD = 1_s;
+
+	static uint8_t CUSTOMER_PASTA_TYPE_PREFERENCE = pasta_information_s::PASTA_TYPE_RIGATONI;
+	static uint8_t CUSTOMER_PASTA_COOKED_TEXTURE_PREFERENCE = pasta_information_s::PASTA_COOKED_AL_DENTE;
+
+	// [deg C] Prefered pasta temperature of the customer
+	static constexpr float CUSTOMER_PASTA_TEMPERATURE_PREFERENCE = 36.5;
+	// [us] Period of the sine curve that will determine the pasta temperature
+	static hrt_abstime PASTA_TEMPERATURE_CURVE_PERIOD = 16_s;
+	// [deg C] Magnitude that will be multiplied to the sine value to calculate temperature
+	static constexpr float PASTA_TEMPERATURE_CURVE_MAGNITUDE = 10.0f;
+
+	static hrt_abstime last_pasta_order_time{0};
+	static uint8_t lasta_pasta_ordered_menu{pasta_information_s::PASTA_MENU_UNDEFINED};
+
+	if ((now - last_pasta_order_time) > PASTA_ORDER_TIME_PERIOD) {
+		pasta_order.timestamp = now;
+
+		// Select MENU: Cycle through the next pasta menu
+		switch (lasta_pasta_ordered_menu) {
+		case pasta_information_s::PASTA_MENU_AGLIO_E_OLIO:
+			pasta_order.menu_name = pasta_information_s::PASTA_MENU_AMATRICIANA;
+			break;
+
+		case pasta_information_s::PASTA_MENU_AMATRICIANA:
+			pasta_order.menu_name = pasta_information_s::PASTA_MENU_CARBONARA;
+			break;
+
+		case pasta_information_s::PASTA_MENU_CARBONARA:
+			pasta_order.menu_name = pasta_information_s::PASTA_MENU_BOLOGNESE;
+			break;
+
+		case pasta_information_s::PASTA_MENU_BOLOGNESE:
+		default:
+			// FALLTHROUGH: If the pasta ordered last time is invalid, order
+			// Aglio e Olio as a default behavior on this loop
+			pasta_order.menu_name = pasta_information_s::PASTA_MENU_AGLIO_E_OLIO;
+		}
+
+		// Set COOKED TEXTURE
+		pasta_order.cooked_texture = CUSTOMER_PASTA_COOKED_TEXTURE_PREFERENCE;
+		// Set TYPE of pasta
+		pasta_order.pasta_type = CUSTOMER_PASTA_TYPE_PREFERENCE;
+
+		// Set TEMPERATURE
+		// It will be a sine curve with a preference value centered, manitude value varying,
+		// period- repeating temperature
+		const float current_phase_rad = ((float)now / PASTA_TEMPERATURE_CURVE_PERIOD) * M_PI_F * 2.0f;
+		const float temperature_adjustment = PASTA_TEMPERATURE_CURVE_MAGNITUDE * sinf(current_phase_rad);
+		pasta_order.pasta_temperature = CUSTOMER_PASTA_TEMPERATURE_PREFERENCE + temperature_adjustment;
+
+		// Update the states
+		last_pasta_order_time = now;
+		lasta_pasta_ordered_menu = pasta_order.menu_name;
+
+		return true;
+	}
+
+	// No order is created
+	return false;
+}
+
+void uORBExplained::waiter_handle_orders(uORB::Subscription &pasta_order_sub,
+		uORB::Publication<pasta_information_s> &pasta_cook_pub)
+{
+	static pasta_information_s customer_order{};
+	// Internal table ID waiter tracks to know to which customer (table) each order belongs to
+	static uint16_t customer_table_id{0};
+
+	// New order from the customer is received
+	if (pasta_order_sub.update(&customer_order)) {
+		// Assign table ID to the customer
+		customer_order.customer_table_id = ++customer_table_id;
+
+		// If the customer isn't sure about the menu, recommend Carbonara!
+		if (customer_order.menu_name == pasta_information_s::PASTA_MENU_UNDEFINED) {
+			customer_order.menu_name = pasta_information_s::PASTA_MENU_CARBONARA;
+		}
+
+		// If customer isn't sure about the cooked texture, recommend the Al Dente!
+		if (customer_order.cooked_texture == pasta_information_s::PASTA_COOKED_UNDEFINED) {
+			customer_order.cooked_texture = pasta_information_s::PASTA_COOKED_AL_DENTE;
+		}
+
+		// If customer isn't sure about the pasta type, recommend Rigatoni!
+		if (customer_order.pasta_type == pasta_information_s::PASTA_TYPE_UNDEFINED) {
+			customer_order.pasta_type = pasta_information_s::PASTA_TYPE_RIGATONI;
+		}
+
+		// Publish so that the chef can receive the order!
+		pasta_cook_pub.publish(customer_order);
+	}
+}
+
+void uORBExplained::chef_handle_orders(uORB::Subscription &pasta_cook_sub)
+{
+	static pasta_information_s waiter_order{};
+
+	// New order from the waiter is received
+	if (pasta_cook_sub.update(&waiter_order)) {
+		PX4_INFO("Chef Cooking new pasta menu %d with temperature %f[deg C]!", waiter_order.menu_name,
+			 (double)waiter_order.pasta_temperature);
+	}
+}
+
+/**
+ * "uorb_explained" module start / stop handling function
+ */
+int uorb_explained_main(int argc, char *argv[])
+{
+	return uORBExplained::main(argc, argv);
+}
+
+uORBExplained::uORBExplained()
+{
+	// Do nothing in the constructor
+}
+
+
+int uORBExplained::task_spawn(int argc, char *argv[])
+{
+	px4_main_t entry_point = (px4_main_t)&run_trampoline;
+
+	_task_id = px4_task_spawn_cmd("uorb_explained",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_DEFAULT,
+				      1500,
+				      entry_point,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		PX4_INFO("uORB Explained module instantiation Failed!");
+		_task_id = -1;
+		return -errno;
+
+	} else {
+		return PX4_OK;
+
+	}
+}
+
+uORBExplained *uORBExplained::instantiate(int argc, char *argv[])
+{
+	return new uORBExplained();
+}
+
+int uORBExplained::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+
+int uORBExplained::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Publishes and subscribes to pasta_cook and pasta_order uORB messages
+
+### Examples
+CLI usage example:
+$ uorb_explained start
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("uuv_att_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start")
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}

src/modules/uorb_explained/uorb_explained.hpp

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. 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.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * Proof of concept module publishing custom uORB topics
+ *
+ * This is a material for the 'uORB Explained' espisode of the PX4 Explained Series
+ * on the px4.io blog.
+ *
+ * @author Junwoo Hwang <junwoo091400@gmail.com>
+ */
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/log.h>
+
+#include <px4_platform_common/module.h>
+
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+
+#include <uORB/topics/pasta_information.h>
+
+using namespace time_literals; // To use time literals like "100_ms"
+
+extern "C" __EXPORT int uorb_explained_main(int argc, char *argv[]);
+
+class uORBExplained : public ModuleBase<uORBExplained>
+{
+public:
+	uORBExplained();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase */
+	static uORBExplained *instantiate(int argc, char *argv[]);
+
+	/**
+	 * @brief Main run function that runs on a thread
+	 * @see ModuleBase
+	 */
+	void run() override;
+
+private:
+	/**
+	 * If a certain time has passed from the last order, fill in the pasta_order with the
+	 * required pasta specification. Returns true if the order has been filled in.
+	 */
+	bool customer_order_pasta(hrt_abstime now, pasta_information_s &pasta_order);
+
+	/**
+	 * Handles Waiters responsibilities: Checking new order from the customer and relaying it
+	 * to the Chef efficiently
+	 */
+	void waiter_handle_orders(uORB::Subscription &pasta_order_sub, uORB::Publication<pasta_information_s> &pasta_cook_pub);
+
+	/**
+	 * Handle Chef's responsibilities: Checking for a new order from the waiter and cooking the pasta!
+	 */
+	void chef_handle_orders(uORB::Subscription &pasta_cook_sub);
+
+	/**
+	 * Publication for the 'pasta_order'
+	 *
+	 * CUSTOMER -> [pasta_cook] -> WAITER
+	 *
+	 * Imaginary topic where a 'customer' orders the 'waiter' to order a pasta.
+	 */
+	uORB::PublicationData<pasta_information_s> _pasta_order_pub{ORB_ID(pasta_order)};
+
+	/**
+	 * Subscription for 'pasta_order', which is done by waiter to receive order from the customer
+	 */
+	uORB::Subscription _pasta_order_sub{ORB_ID(pasta_order)};
+
+	/**
+	 * Publication for the 'pasta_cook'
+	 *
+	 * WAITER -> [pasta_cook] -> CHEF
+	 *
+	 * Imaginary topic where a 'waiter' orders the 'chef' to cook a certain pasta.
+	 */
+	uORB::Publication<pasta_information_s> _pasta_cook_pub{ORB_ID(pasta_cook)};
+
+	/**
+	 * Subscription for 'pasta_cook', which is done by the chef to receive order from the waiter
+	 */
+	uORB::Subscription _pasta_cook_sub{ORB_ID(pasta_cook)};
+};