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/message_version.hpp>
#include <px4_ros2/utils/subscription.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 */