diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3fae8ea8bd..7511373fad 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -137,6 +137,7 @@ using matrix::wrap_2pi; #include "streams/STATUSTEXT.hpp" #include "streams/STORAGE_INFORMATION.hpp" #include "streams/SYSTEM_TIME.hpp" +#include "streams/SYS_STATUS.hpp" #include "streams/TIMESYNC.hpp" #include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" @@ -558,116 +559,6 @@ protected: } }; -class MavlinkStreamSysStatus : public MavlinkStream -{ -public: - const char *get_name() const override - { - return MavlinkStreamSysStatus::get_name_static(); - } - - static constexpr const char *get_name_static() - { - return "SYS_STATUS"; - } - - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_SYS_STATUS; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamSysStatus(mavlink); - } - - unsigned get_size() override - { - return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; - - /* do not allow top copying this class */ - MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; - MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &) = delete; - -protected: - explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - } - - bool send() override - { - if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) { - vehicle_status_s status{}; - _status_sub.copy(&status); - - cpuload_s cpuload{}; - _cpuload_sub.copy(&cpuload); - - battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; - - for (int i = 0; i < _battery_status_subs.size(); i++) { - _battery_status_subs[i].copy(&battery_status[i]); - } - - int lowest_battery_index = 0; - - // No battery is connected, select the first group - // Low battery judgment is performed only when the current battery is connected - // When the last cached battery is not connected or the current battery level is lower than the cached battery level, - // the current battery status is replaced with the cached value - for (int i = 0; i < _battery_status_subs.size(); i++) { - if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) - || (battery_status[i].remaining < battery_status[lowest_battery_index].remaining))) { - lowest_battery_index = i; - } - } - - mavlink_sys_status_t msg{}; - - msg.onboard_control_sensors_present = status.onboard_control_sensors_present; - msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; - msg.onboard_control_sensors_health = status.onboard_control_sensors_health; - - msg.load = cpuload.load * 1000.0f; - - // TODO: Determine what data should be put here when there are multiple batteries. - // Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking - // one battery using this message, it should be the lowest. - // In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS - // to determine which battery is more important at a given time. - const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; - - if (lowest_battery.connected) { - msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; - msg.current_battery = lowest_battery.current_filtered_a * 100.0f; - msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); - - } else { - msg.voltage_battery = UINT16_MAX; - msg.current_battery = -1; - msg.battery_remaining = -1; - } - - mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - class MavlinkStreamBatteryStatus : public MavlinkStream { public: @@ -978,7 +869,9 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // STATUSTEXT_HPP create_stream_list_item(), +#if defined(SYSTEM_TIME_HPP) create_stream_list_item(), +#endif // SYSTEM_TIME_HPP create_stream_list_item(), #if defined(SMART_BATTERY_INFO_HPP) create_stream_list_item(), diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp new file mode 100644 index 0000000000..4fe5d8e046 --- /dev/null +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 SYS_STATUS_HPP +#define SYS_STATUS_HPP + +#include +#include +#include + +class MavlinkStreamSysStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSysStatus(mavlink); } + + static constexpr const char *get_name_static() { return "SYS_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SYS_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + + bool send() override + { + if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) { + vehicle_status_s status{}; + _status_sub.copy(&status); + + cpuload_s cpuload{}; + _cpuload_sub.copy(&cpuload); + + battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; + + for (int i = 0; i < _battery_status_subs.size(); i++) { + _battery_status_subs[i].copy(&battery_status[i]); + } + + int lowest_battery_index = 0; + + // No battery is connected, select the first group + // Low battery judgment is performed only when the current battery is connected + // When the last cached battery is not connected or the current battery level is lower than the cached battery level, + // the current battery status is replaced with the cached value + for (int i = 0; i < _battery_status_subs.size(); i++) { + if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) + || (battery_status[i].remaining < battery_status[lowest_battery_index].remaining))) { + lowest_battery_index = i; + } + } + + mavlink_sys_status_t msg{}; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + + msg.load = cpuload.load * 1000.0f; + + // TODO: Determine what data should be put here when there are multiple batteries. + // Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking + // one battery using this message, it should be the lowest. + // In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS + // to determine which battery is more important at a given time. + const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; + + if (lowest_battery.connected) { + msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; + msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); + + } else { + msg.voltage_battery = UINT16_MAX; + msg.current_battery = -1; + msg.battery_remaining = -1; + } + + mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // SYS_STATUS_HPP