battery.hpp

/****************************************************************************
 * Copyright (c) 2023-2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/battery_status.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/utils/subscription.hpp>
#include <px4_ros2/utils/message_version.hpp>
 
namespace px4_ros2
{
class Battery : public Subscription<px4_msgs::msg::BatteryStatus>
{
public:
  explicit Battery(Context & context)
  : Subscription<px4_msgs::msg::BatteryStatus>(context,
      "fmu/out/battery_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::BatteryStatus>()) {}
 
  float voltageV() const
  {
    return last().voltage_v;
  }
 
  float currentA() const
  {
    return last().current_a;
  }
 
  float dischargedCapacityMah() const
  {
    return last().discharged_mah;
  }
 
  float remaningFraction() const
  {
    return last().remaining;
  }
 
  uint8_t cellCount() const
  {
    return last().cell_count;
  }
 
  Eigen::VectorXf cellVoltagesV() const
  {
    const px4_msgs::msg::BatteryStatus & battery = last();
    const uint8_t cell_count = battery.cell_count;
 
    if (cell_count == 0) {
      RCLCPP_WARN_THROTTLE(
        _node.get_logger(),
        *_node.get_clock(), 1000, "Failed to retrieve battery cell voltage: cell count unknown.");
      return Eigen::VectorXf();
    }
 
    Eigen::VectorXf cell_voltages(cell_count);
    for (uint8_t i = 0; i < cell_count; ++i) {
      cell_voltages(i) = battery.voltage_cell_v[i];
    }
 
    return cell_voltages;
  }
 
};
 
} /* namespace px4_ros2 */