From a8cb5a7715d605d80c812a810398c5060f1c8233 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Wed, 15 May 2024 16:06:48 +0100 Subject: [PATCH] Add fuel tank status report support * Adds support to DroneCAN FuelTankStatus messages * Adds fuel_tank_status uORB message * Adds FUEL_STATUS MAVLink stream * Adds parameter to define max fuel tank capacity --- msg/CMakeLists.txt | 1 + msg/FuelTankStatus.msg | 17 ++++ src/drivers/uavcan/CMakeLists.txt | 1 + src/drivers/uavcan/Kconfig | 4 + .../uavcan/sensors/fuel_tank_status.cpp | 94 +++++++++++++++++++ .../uavcan/sensors/fuel_tank_status.hpp | 70 ++++++++++++++ src/drivers/uavcan/sensors/sensor_bridge.cpp | 14 +++ src/drivers/uavcan/uavcan_params.c | 25 +++++ src/lib/parameters/px4params/srcparser.py | 2 +- src/modules/logger/logged_topics.cpp | 1 + src/modules/mavlink/mavlink_main.cpp | 5 + src/modules/mavlink/mavlink_messages.cpp | 4 + src/modules/mavlink/streams/FUEL_STATUS.hpp | 86 +++++++++++++++++ 13 files changed, 323 insertions(+), 1 deletion(-) create mode 100644 msg/FuelTankStatus.msg create mode 100644 src/drivers/uavcan/sensors/fuel_tank_status.cpp create mode 100644 src/drivers/uavcan/sensors/fuel_tank_status.hpp create mode 100644 src/modules/mavlink/streams/FUEL_STATUS.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 53ec65cf9a..a3df406a9c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -98,6 +98,7 @@ set(msg_files FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg + FuelTankStatus.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg diff --git a/msg/FuelTankStatus.msg b/msg/FuelTankStatus.msg new file mode 100644 index 0000000000..cddd8b73d5 --- /dev/null +++ b/msg/FuelTankStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +float32 maximum_fuel_capacity # maximum fuel capacity +float32 consumed_fuel # consumed fuel, NaN if unavailable +float32 fuel_consumption_rate # fuel consumption rate + +uint8 percent_remaining # percentage of remaining fuel +float32 remaining_fuel # remaining fuel + +uint8 fuel_tank_id # identifier for the fuel tank + +uint32 fuel_type # type of fuel based on MAV_FUEL_TYPE enum +uint8 MAV_FUEL_TYPE_UNKNOWN = 0 # fuel type not specified. Fuel levels are normalized (i.e., maximum is 1, and other levels are relative to 1). +uint8 MAV_FUEL_TYPE_LIQUID = 1 # represents generic liquid fuels, such as gasoline or diesel. Fuel levels are measured in millilitres (ml), and flow rates in millilitres per second (ml/s). +uint8 MAV_FUEL_TYPE_GAS = 2 # represents a gas fuel, such as hydrogen, methane, or propane. Fuel levels are in kilo-Pascal (kPa), and flow rates are in milliliters per second (ml/s). + +float32 temperature # fuel temperature in Kelvin, NaN if not measured diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 39a9bb3ff2..c70d029ea7 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -168,6 +168,7 @@ px4_add_module( sensors/battery.cpp sensors/airspeed.cpp sensors/flow.cpp + sensors/fuel_tank_status.cpp sensors/gnss_relative.cpp sensors/gnss.cpp sensors/mag.cpp diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index 015894eb6e..0076d36568 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -54,6 +54,10 @@ if DRIVERS_UAVCAN bool "Subscribe to Flow: com::hex::equipment::flow::Measurement" default y + config UAVCAN_SENSOR_FUEL_TANK_STATUS + bool "Subscribe to Fuel Tank Status: uavcan::equipment::ice::FuelTankStatus" + default y + config UAVCAN_SENSOR_GNSS bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" default y diff --git a/src/drivers/uavcan/sensors/fuel_tank_status.cpp b/src/drivers/uavcan/sensors/fuel_tank_status.cpp new file mode 100644 index 0000000000..2f0fb484bb --- /dev/null +++ b/src/drivers/uavcan/sensors/fuel_tank_status.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * @file fuel_tank_status.cpp + * @author Nuno Marques + */ + +#include "fuel_tank_status.hpp" + +#include +#include + +const char *const UavcanFuelTankStatusBridge::NAME = "fuel_tank_status"; + +UavcanFuelTankStatusBridge::UavcanFuelTankStatusBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_fuel_tank_status", ORB_ID(fuel_tank_status)), + _sub_fuel_tank_status_data(node) +{ } + +int UavcanFuelTankStatusBridge::init() +{ + int res = _sub_fuel_tank_status_data.start(FuelTankStatusCbBinder(this, + &UavcanFuelTankStatusBridge::fuel_tank_status_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void UavcanFuelTankStatusBridge::fuel_tank_status_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + auto report = ::fuel_tank_status_s(); + report.timestamp = hrt_absolute_time(); + + // Fetching maximum fuel capacity (in liters) from a parameter + param_get(param_find("UAVCAN_ECU_MAXF"), &_max_fuel_capacity); + + _max_fuel_capacity *= 1000.0f; + report.maximum_fuel_capacity = _max_fuel_capacity; + report.fuel_type = fuel_tank_status_s::MAV_FUEL_TYPE_LIQUID; + + // Calculating consumed fuel based on available fuel + report.consumed_fuel = (_max_fuel_capacity > msg.available_fuel_volume_cm3) ? _max_fuel_capacity - + msg.available_fuel_volume_cm3 : NAN; + report.fuel_consumption_rate = msg.fuel_consumption_rate_cm3pm / 60.0f; // convert to ml/s + report.percent_remaining = msg.available_fuel_volume_percent; + report.remaining_fuel = msg.available_fuel_volume_cm3; + report.fuel_tank_id = msg.fuel_tank_id; + + // Optional temperature field, in Kelvin, set to NaN if not provided. + report.temperature = !PX4_ISFINITE(msg.fuel_temperature) ? NAN : msg.fuel_temperature; + + publish(msg.getSrcNodeID().get(), &report); +} + +int UavcanFuelTankStatusBridge::init_driver(uavcan_bridge::Channel *channel) +{ + return PX4_OK; +} diff --git a/src/drivers/uavcan/sensors/fuel_tank_status.hpp b/src/drivers/uavcan/sensors/fuel_tank_status.hpp new file mode 100644 index 0000000000..49f43efb68 --- /dev/null +++ b/src/drivers/uavcan/sensors/fuel_tank_status.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * @file fuel_tank_status.hpp + * @author Nuno Marques + * @brief UAVCAN bridge for Fuel Tank Status messages. + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include + +class UavcanFuelTankStatusBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanFuelTankStatusBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void fuel_tank_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + int init_driver(uavcan_bridge::Channel *channel) override; + + typedef uavcan::MethodBinder &)> + FuelTankStatusCbBinder; + + uavcan::Subscriber _sub_fuel_tank_status_data; + + float _max_fuel_capacity{0.0f}; +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index c219d90f3f..1fa67050e1 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -57,6 +57,9 @@ #if defined(CONFIG_UAVCAN_SENSOR_FLOW) #include "flow.hpp" #endif +#if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS) +#include "fuel_tank_status.hpp" +#endif #if defined(CONFIG_UAVCAN_SENSOR_GNSS) #include "gnss.hpp" #endif @@ -137,6 +140,17 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List(), #endif // FLIGHT_INFORMATION_HPP +#if defined(FUEL_STATUS_HPP) + create_stream_list_item(), +#endif // FUEL_STATUS_HPP #if defined(GPS_STATUS_HPP) create_stream_list_item(), #endif // GPS_STATUS_HPP diff --git a/src/modules/mavlink/streams/FUEL_STATUS.hpp b/src/modules/mavlink/streams/FUEL_STATUS.hpp new file mode 100644 index 0000000000..350f9c9fc7 --- /dev/null +++ b/src/modules/mavlink/streams/FUEL_STATUS.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#ifndef FUEL_STATUS_HPP +#define FUEL_STATUS_HPP + +#include + +class MavlinkStreamFuelStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamFuelStatus(mavlink); } + + static constexpr const char *get_name_static() { return "FUEL_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FUEL_STATUS; } + + const char *get_name() const override { return MavlinkStreamFuelStatus::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _fuel_tank_status_sub.advertised() ? MAVLINK_MSG_ID_FUEL_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamFuelStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _fuel_tank_status_sub{ORB_ID(fuel_tank_status)}; + + bool send() override + { + fuel_tank_status_s fuel_status; + + if (_fuel_tank_status_sub.update(&fuel_status)) { + mavlink_fuel_status_t msg{}; + + msg.id = fuel_status.fuel_tank_id; + msg.maximum_fuel = fuel_status.maximum_fuel_capacity; + msg.consumed_fuel = fuel_status.consumed_fuel; + msg.remaining_fuel = fuel_status.remaining_fuel; + msg.percent_remaining = fuel_status.percent_remaining; + msg.flow_rate = fuel_status.fuel_consumption_rate; + msg.temperature = fuel_status.temperature; + msg.fuel_type = fuel_status.fuel_type; + + mavlink_msg_fuel_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } + +}; + +#endif // FUEL_STATUS_HPP