fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 21:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add BATTERY_STATUS_V2 streaming

Browse Source
This commit is contained in:
Hamish Willee
2022-05-22 22:21:04 +10:00
parent 5ca0862bbb
commit e7fa130b06
4 changed files with 140 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/battery_status.msg
+23
View File
@@ -76,3 +76,26 @@ float32 design_capacity # The design capacity of the battery
uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes
uint16 over_discharge_count # Number of battery overdischarge
float32 nominal_voltage # Nominal voltage of the battery pack
uint32 status_flags # Battery status flags
# Battery status flags (combined faults and modes)
uint32 STATUS_FLAG_READY = 1 # The battery is ready to use (fly).
uint32 STATUS_FLAG_CHARGING = 2 # Battery is charging.
uint32 STATUS_FLAG_CELL_BALANCING = 4 # Battery is cell balancing (during charging).
uint32 STATUS_FLAG_FAULT_CELL_IMBALANCE = 8 # Battery cells are not balanced.
uint32 STATUS_FLAG_AUTO_DISCHARGING = 16 # Battery is auto discharging (towards storage level).
uint32 STATUS_FLAG_REQUIRES_SERVICE = 32 # Battery requires service (not safe to fly).
uint32 STATUS_FLAG_BAD_BATTERY = 64 # Battery is faulty and cannot be repaired (not safe to fly).
uint32 STATUS_FLAG_PROTECTIONS_ENABLED = 128 # Automatic battery protection monitoring is enabled.
uint32 STATUS_FLAG_FAULT_PROTECTION_SYSTEM = 256 # The battery fault protection system had detected a fault and cut all power from the battery.
uint32 STATUS_FLAG_FAULT_OVER_VOLT = 512 # One or more cells are above their maximum voltage rating.
uint32 STATUS_FLAG_FAULT_UNDER_VOLT = 1024 # One or more cells are below their minimum voltage rating.
uint32 STATUS_FLAG_FAULT_OVER_TEMPERATURE = 2048 # Over-temperature fault.
uint32 STATUS_FLAG_FAULT_UNDER_TEMPERATURE = 4096 # Under-temperature fault.
uint32 STATUS_FLAG_FAULT_OVER_CURRENT = 8192 # Over-current fault.
uint32 STATUS_FLAG_FAULT_SHORT_CIRCUIT = 16384 # Short circuit event detected.
uint32 STATUS_FLAG_FAULT_INCOMPATIBLE_VOLTAGE = 32768 # Voltage not compatible with power rail voltage.
uint32 STATUS_FLAG_FAULT_INCOMPATIBLE_FIRMWARE = 65536 # Battery firmware is not compatible with current autopilot firmware.
uint32 STATUS_FLAG_FAULT_INCOMPATIBLE_CELLS_CONFIGURATION = 131072 # Battery is not compatible due to cell configuration (e.g. 5s1p when vehicle requires 6s).
src/modules/mavlink/mavlink_main.cpp
+6
View File
@@ -1492,6 +1492,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE", 15.0f);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DISTANCE_SENSOR", 0.5f);
@@ -1556,6 +1557,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("EFI_STATUS", 2.0f);
@@ -1623,6 +1625,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
@@ -1663,6 +1666,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE", 25.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
@@ -1701,6 +1705,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
configure_stream_local("ATTITUDE_TARGET", 8.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("EFI_STATUS", 10.0f);
@@ -1789,6 +1794,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("BATTERY_STATUS_V2", 0.5f);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
src/modules/mavlink/mavlink_messages.cpp
+6
View File
@@ -64,6 +64,7 @@
#include "streams/ATTITUDE_TARGET.hpp"
#include "streams/AUTOPILOT_VERSION.hpp"
#include "streams/BATTERY_STATUS.hpp"
#include "streams/BATTERY_STATUS_V2.hpp"
#include "streams/CAMERA_IMAGE_CAPTURED.hpp"
#include "streams/CAMERA_TRIGGER.hpp"
#include "streams/COLLISION.hpp"
@@ -328,7 +329,12 @@ static const StreamListItem streams_list[] = {
#if defined(SYSTEM_TIME_HPP)
create_stream_list_item<MavlinkStreamSysStatus>(),
#endif // SYSTEM_TIME_HPP
#if defined(BATTERY_STATUS_HPP)
create_stream_list_item<MavlinkStreamBatteryStatus>(),
#endif // BATTERY_STATUS_HPP
#if defined(BATTERY_STATUS_V2_HPP)
create_stream_list_item<MavlinkStreamBatteryStatusV2>(),
#endif // BATTERY_STATUS_V2_HPP
#if defined(SMART_BATTERY_INFO_HPP)
create_stream_list_item<MavlinkStreamSmartBatteryInfo>(),
#endif // SMART_BATTERY_INFO_HPP
src/modules/mavlink/streams/BATTERY_STATUS_V2.hpp
+105
View File
@@ -0,0 +1,105 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#ifndef BATTERY_STATUS_V2_HPP
#define BATTERY_STATUS_V2_HPP
#include <uORB/topics/battery_status.h>
class MavlinkStreamBatteryStatusV2 : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamBatteryStatusV2(mavlink); }
static constexpr const char *get_name_static() { return "BATTERY_STATUS_V2"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_BATTERY_STATUS_V2; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_STATUS_V2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return size_per_battery * _battery_status_subs.advertised_count();
}
private:
explicit MavlinkStreamBatteryStatusV2(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
bool send() override
{
bool updated = false;
for (auto &battery_sub : _battery_status_subs) {
battery_status_s battery_status;
if (battery_sub.update(&battery_status)) {
/* battery status message with higher resolution */
mavlink_battery_status_v2_t bat_msg{};
// TODO: Determine how to better map between battery ID within the firmware and in MAVLink
bat_msg.id = battery_status.id - 1;
bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : UINT32_MAX;
bat_msg.current = (battery_status.connected) ? battery_status.current_filtered_a * 1000 : UINT32_MAX; // mA
bat_msg.percent_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : UINT32_MAX;
bat_msg.status_flags = battery_status.status_flags;
bat_msg.voltage = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.f : UINT32_MAX;
// get remaining charge in mAH
if (battery_status.connected && PX4_ISFINITE(battery_status.remaining_capacity_wh) && (battery_status.voltage_filtered_v > 0.f) ) {
bat_msg.current_remaining = (battery_status.remaining_capacity_wh / battery_status.voltage_filtered_v) * 1000.f; // mAH
} else {
bat_msg.current_remaining = UINT32_MAX;
}
// check if temperature valid
if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) {
bat_msg.temperature = battery_status.temperature * 100.f;
} else {
bat_msg.temperature = INT16_MAX;
}
mavlink_msg_battery_status_v2_send_struct(_mavlink->get_channel(), &bat_msg);
updated = true;
}
}
return updated;
}
};
#endif // BATTERY_STATUS_V2_HPP