fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add fuel tank status report support

Browse Source 
* 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
This commit is contained in:
Nuno Marques 2024-05-15 16:06:48 +01:00
parent 954225a5c0
commit a8cb5a7715
13 changed files with 323 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -98,6 +98,7 @@ set(msg_files
FollowTarget.msg
FollowTargetEstimator.msg
FollowTargetStatus.msg
FuelTankStatus.msg
GeneratorStatus.msg
GeofenceResult.msg
GeofenceStatus.msg

17
msg/FuelTankStatus.msg Normal file
View File

@ -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

1
src/drivers/uavcan/CMakeLists.txt
View File

@ -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

4
src/drivers/uavcan/Kconfig
View File

@ -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

94
src/drivers/uavcan/sensors/fuel_tank_status.cpp Normal file
View File

@ -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 <n.marques21@hotmail.com>
*/
#include "fuel_tank_status.hpp"
#include <parameters/param.h>
#include <uORB/topics/fuel_tank_status.h>
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<uavcan::equipment::ice::FuelTankStatus> &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;
}

70
src/drivers/uavcan/sensors/fuel_tank_status.hpp Normal file
View File

@ -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 <n.marques21@hotmail.com>
* @brief UAVCAN bridge for Fuel Tank Status messages.
*/
#pragma once
#include "sensor_bridge.hpp"
#include <uavcan/equipment/ice/FuelTankStatus.hpp>
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<uavcan::equipment::ice::FuelTankStatus> &msg);
int init_driver(uavcan_bridge::Channel *channel) override;
typedef uavcan::MethodBinder<UavcanFuelTankStatusBridge *,
void (UavcanFuelTankStatusBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ice::FuelTankStatus> &)>
FuelTankStatusCbBinder;
uavcan::Subscriber<uavcan::equipment::ice::FuelTankStatus, FuelTankStatusCbBinder> _sub_fuel_tank_status_data;
float _max_fuel_capacity{0.0f};
};

14
src/drivers/uavcan/sensors/sensor_bridge.cpp
View File

@ -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<IUavcanSensorBridge
list.add(new UavcanFlowBridge(node));
}
#endif
// fuel tank
#if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS)
int32_t uavcan_sub_fuel_tank = 1;
param_get(param_find("UAVCAN_SUB_FUEL"), &uavcan_sub_fuel_tank);
if (uavcan_sub_fuel_tank != 0) {
list.add(new UavcanFuelTankStatusBridge(node));
}
#endif
// GPS

25
src/drivers/uavcan/uavcan_params.c
View File

@ -97,6 +97,20 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f);
*/
PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f);
/**
* UAVCAN fuel tank maximum capacity
*
* This parameter defines the maximum fuel capacity of the vehicle's fuel tank.
*
* @min 0.0
* @max 100000.0
* @unit liters
* @decimal 1
* @increment 0.1
* @group UAVCAN
*/
PARAM_DEFINE_FLOAT(UAVCAN_ECU_MAXF, 15.0f);
/**
* UAVCAN ANTI_COLLISION light operating mode
*
@ -292,6 +306,17 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_DPRES, 0);
*/
PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0);
/**
* subscription fuel tank
*
* Enable UAVCAN fuel tank status subscription.
*
* @boolean
* @reboot_required true
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_SUB_FUEL, 0);
/**
* subscription GPS
*

2
src/lib/parameters/px4params/srcparser.py
View File

@ -356,7 +356,7 @@ class SourceParser(object):
'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m',
'bit/s', 'B/s',
'deg', 'deg*1e7', 'deg/s', 'deg/s^2',
'celcius', 'gauss', 'gauss/s', 'gauss^2',
'celcius', 'gauss', 'gauss/s', 'gauss^2', 'liters',
'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', 'g0',
'Ohm', 'V', 'A',

1
src/modules/logger/logged_topics.cpp
View File

@ -71,6 +71,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("follow_target_status", 400);
add_optional_topic("flaps_setpoint", 1000);
add_optional_topic("flight_phase_estimation", 1000);
add_optional_topic("fuel_tank_status", 10);
add_topic("gimbal_manager_set_attitude", 500);
add_optional_topic("generator_status");
add_optional_topic("gps_dump");

5
src/modules/mavlink/mavlink_main.cpp
View File

@ -1413,6 +1413,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ESC_STATUS", 1.0f);
configure_stream_local("ESTIMATOR_STATUS", 0.5f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("FUEL_STATUS", 1.0f);
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
@ -1482,6 +1483,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EFI_STATUS", 2.0f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
configure_stream_local("FUEL_STATUS", 1.0f);
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
@ -1556,6 +1558,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("CURRENT_MODE", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("FUEL_STATUS", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GPS2_RAW", 1.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
@ -1642,6 +1645,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ESC_STATUS", 10.0f);
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("FUEL_STATUS", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
@ -1738,6 +1742,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("CURRENT_MODE", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("FUEL_STATUS", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);

4
src/modules/mavlink/mavlink_messages.cpp
View File

@ -75,6 +75,7 @@
#include "streams/ESC_STATUS.hpp"
#include "streams/ESTIMATOR_STATUS.hpp"
#include "streams/EXTENDED_SYS_STATE.hpp"
#include "streams/FUEL_STATUS.hpp"
#include "streams/FLIGHT_INFORMATION.hpp"
#include "streams/GLOBAL_POSITION_INT.hpp"
#include "streams/GPS_GLOBAL_ORIGIN.hpp"
@ -469,6 +470,9 @@ static const StreamListItem streams_list[] = {
#if defined(FLIGHT_INFORMATION_HPP)
create_stream_list_item<MavlinkStreamFlightInformation>(),
#endif // FLIGHT_INFORMATION_HPP
#if defined(FUEL_STATUS_HPP)
create_stream_list_item<MavlinkStreamFuelStatus>(),
#endif // FUEL_STATUS_HPP
#if defined(GPS_STATUS_HPP)
create_stream_list_item<MavlinkStreamGPSStatus>(),
#endif // GPS_STATUS_HPP

86
src/modules/mavlink/streams/FUEL_STATUS.hpp Normal file
View File

@ -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 <uORB/topics/fuel_tank_status.h>
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