Daniel Agar
4 years ago
2 changed files with 129 additions and 110 deletions
@ -0,0 +1,126 @@
@@ -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 <uORB/topics/battery_status.h> |
||||
#include <uORB/topics/cpuload.h> |
||||
#include <uORB/topics/vehicle_status.h> |
||||
|
||||
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_s, battery_status_s::MAX_INSTANCES> _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
|
||||
Loading…
Reference in new issue