text: /****************************************************************************
   *
   *   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.
   *
   ****************************************************************************/
  
  #include "cbat.hpp"
  
  #include <lib/geo/geo.h>
  #include <px4_defines.h>
  
  const char *const UavcanCBATBridge::NAME = "cbat";
  
  UavcanCBATBridge::UavcanCBATBridge(uavcan::INode &node) :
  	UavcanSensorBridgeBase("uavcan_cbat", ORB_ID(battery_status)),
  	ModuleParams(nullptr),
  	_sub_battery(node),
  	_warning(battery_status_s::BATTERY_WARNING_NONE)
  {
  }
  
  int UavcanCBATBridge::init()
  {
  	int res = _sub_battery.start(CBATCbBinder(this, &UavcanCBATBridge::battery_sub_cb));
  
  	if (res < 0) {
  		PX4_ERR("failed to start uavcan sub: %d", res);
  		return res;
  	}
  
  	return 0;
  }
  
  void
  UavcanCBATBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
  {
  	battery_status_s battery{};
  
  	battery.timestamp = hrt_absolute_time();
  	battery.voltage_v = msg.voltage;
  	battery.voltage_filtered_v = battery.voltage_v;
  	battery.current_a = msg.current;
  	battery.current_filtered_a = battery.current_a;
  	battery.current_average_a = msg.average_current;
  	battery.discharged_mah = msg.passed_charge * 1000;
  	battery.remaining = msg.state_of_charge / 100.0f;
  	// battery.scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
  	battery.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
  	battery.cell_count = msg.cell_count;
  	battery.connected = true;
  	battery.source = msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_IN_USE; // BATTERY_SOURCE_EXTERNAL
  	// battery.priority = msg.;
  	battery.capacity = msg.full_charge_capacity * 1000;
  	battery.cycle_count = msg.cycle_count;
  	battery.time_remaining_s = msg.average_time_to_empty * 60;
  	battery.average_time_to_empty = msg.average_time_to_empty;
  	battery.serial_number = msg.serial_number;
  	battery.manufacture_date = msg.manufacture_date;
  	battery.state_of_health = msg.state_of_health;
  	battery.max_error = msg.max_error;
  	battery.id = msg.getSrcNodeID().get();
  	battery.interface_error = msg.interface_error;
  
  	for (uint8_t i = 0; i < msg.cell_count; i++) {
  		battery.voltage_cell_v[i] = msg.voltage_cell[i];
  	}
  
  	//Calculate max cell delta
  	float min_cell_voltage = msg.voltage_cell[0];
  	float max_cell_voltage = msg.voltage_cell[0];
  
  	for (uint8_t i = 1; i < msg.cell_count; i++) {
  		min_cell_voltage = math::min(min_cell_voltage, msg.voltage_cell[i]);
  		max_cell_voltage = math::max(max_cell_voltage, msg.voltage_cell[i]);
  	}
  
  	// Calculate the max difference between the min and max cells with complementary filter.
  	battery.max_cell_voltage_delta = (0.5f * (max_cell_voltage - min_cell_voltage)) +
  					 (0.5f * _max_cell_voltage_delta);
  	_max_cell_voltage_delta = battery.max_cell_voltage_delta;
  
  	battery.is_powering_off = msg.is_powering_off;
  
  	determineWarning(battery.remaining);
  	battery.warning = _warning;
  
  	// Expand the information
  	battery.average_power = msg.average_power;
  	battery.available_energy = msg.available_energy;
  	battery.remaining_capacity = msg.remaining_capacity;
  	battery.design_capacity = msg.design_capacity;
  	battery.average_time_to_full = msg.average_time_to_full;
  	battery.over_discharge_count = msg.over_discharge_count;
  	battery.nominal_voltage = msg.nominal_voltage;
  
  	publish(msg.getSrcNodeID().get(), &battery);
  }
  
  void
  UavcanCBATBridge::determineWarning(float remaining)
  {
  	// propagate warning state only if the state is higher, otherwise remain in current warning state
  	if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
  		_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
  
  	} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
  		_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
  
  	} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
  		_warning = battery_status_s::BATTERY_WARNING_LOW;
  	}
  }
  