diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 882e408250..a9f1de9de6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -267,3 +267,5 @@ fi mavlink boot_complete replay trystart + +uorb_explained start diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2064d91ba2..52d70fcc63 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/pasta_information.msg b/msg/pasta_information.msg new file mode 100644 index 0000000000..a3b2a15abf --- /dev/null +++ b/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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1a0840006d..bc1a9d12ef 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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"); diff --git a/src/modules/uorb_explained/CMakeLists.txt b/src/modules/uorb_explained/CMakeLists.txt new file mode 100644 index 0000000000..6ac7270637 --- /dev/null +++ b/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 + ) diff --git a/src/modules/uorb_explained/Kconfig b/src/modules/uorb_explained/Kconfig new file mode 100644 index 0000000000..4106d0c4c7 --- /dev/null +++ b/src/modules/uorb_explained/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_UORB_EXPLAINED + bool "uorb_explained module" + default y + ---help--- + uORB Explained Module diff --git a/src/modules/uorb_explained/uorb_explained.cpp b/src/modules/uorb_explained/uorb_explained.cpp new file mode 100644 index 0000000000..237ef61f39 --- /dev/null +++ b/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_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; +} diff --git a/src/modules/uorb_explained/uorb_explained.hpp b/src/modules/uorb_explained/uorb_explained.hpp new file mode 100644 index 0000000000..1b296f0ea7 --- /dev/null +++ b/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 + */ + +#include +#include + +#include + +#include +#include +#include + +#include + +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 +{ +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_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_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_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)}; +};