Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: replace libuavcan with libcanard based driver

Browse Source
This commit is contained in:
bugobliterator 2023-01-04 22:11:32 +11:00 committed by Andrew Tridgell
parent 0d90e0377a
commit 6a1460efb0
2 changed files with 57 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
View File

@ -13,11 +13,6 @@
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
#include <mppt/Stream.hpp>
#include <mppt/OutputEnable.hpp>
#define LOG_TAG "BattMon"
extern const AP_HAL::HAL& hal;
@ -36,13 +31,6 @@ const AP_Param::GroupInfo AP_BattMonitor_UAVCAN::var_info[] = {
AP_GROUPEND
};
UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo);
UC_REGISTRY_BINDER(BattInfoAuxCb, ardupilot::equipment::power::BatteryInfoAux);
UC_REGISTRY_BINDER(MpptStreamCb, mppt::Stream);
static void trampoline_handleOutputEnable(const uavcan::ServiceCallResult<mppt::OutputEnable>& resp);
static uavcan::ServiceClient<mppt::OutputEnable>* outputEnable_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
/// Constructor
AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params) :
AP_BattMonitor_Backend(mon, mon_state, params),
@ -61,43 +49,17 @@ void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
return;
}
auto* node = ap_uavcan->get_node();
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb> *battinfo_listener;
battinfo_listener = new uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb>(*node);
// Backend Msg Handler
const int battinfo_listener_res = battinfo_listener->start(BattInfoCb(ap_uavcan, &handle_battery_info_trampoline));
if (battinfo_listener_res < 0) {
AP_BoardConfig::allocation_error("UAVCAN BatteryInfo subscriber start problem\n\r");
return;
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_battery_info_trampoline, ap_uavcan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("battinfo_sub");
}
uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BattInfoAuxCb> *battinfo_aux_listener;
battinfo_aux_listener = new uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BattInfoAuxCb>(*node);
// Backend Msg Handler
const int battinfo_aux_listener_res = battinfo_aux_listener->start(BattInfoAuxCb(ap_uavcan, &handle_battery_info_aux_trampoline));
if (battinfo_aux_listener_res < 0) {
AP_BoardConfig::allocation_error("UAVCAN BatteryInfoAux subscriber start problem");
return;
}
uavcan::Subscriber<mppt::Stream, MpptStreamCb> *mppt_stream_listener;
mppt_stream_listener = new uavcan::Subscriber<mppt::Stream, MpptStreamCb>(*node);
// Backend Msg Handler
const int mppt_stream_listener_res = mppt_stream_listener->start(MpptStreamCb(ap_uavcan, &handle_mppt_stream_trampoline));
if (mppt_stream_listener_res < 0) {
AP_BoardConfig::allocation_error("UAVCAN Mppt::Stream subscriber start problem");
return;
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_battery_info_aux_trampoline, ap_uavcan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("battinfo_aux_sub");
}
// set up callback to verify mppt::OutputEnable
const uint8_t driver_index = ap_uavcan->get_driver_index();
outputEnable_client[driver_index] = new uavcan::ServiceClient<mppt::OutputEnable>(*node);
if (outputEnable_client[driver_index] == nullptr || outputEnable_client[driver_index]->init() < 0) {
AP_BoardConfig::allocation_error("UAVCAN Mppt::outputEnable client start problem");
return;
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_mppt_stream_trampoline, ap_uavcan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("mppt_stream_sub");
}
outputEnable_client[driver_index]->setCallback(trampoline_handleOutputEnable);
}
AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id)
@ -128,7 +90,6 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
batmon->_ap_uavcan = ap_uavcan;
batmon->_node_id = node_id;
batmon->_instance = i;
batmon->_node = ap_uavcan->get_node();
batmon->init();
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
@ -141,13 +102,13 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
return nullptr;
}
void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
void AP_BattMonitor_UAVCAN::handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg)
{
update_interim_state(cb.msg->voltage, cb.msg->current, cb.msg->temperature, cb.msg->state_of_charge_pct);
update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct);
WITH_SEMAPHORE(_sem_battmon);
_remaining_capacity_wh = cb.msg->remaining_capacity_wh;
_full_charge_capacity_wh = cb.msg->full_charge_capacity_wh;
_remaining_capacity_wh = msg.remaining_capacity_wh;
_full_charge_capacity_wh = msg.full_charge_capacity_wh;
}
void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc)
@ -178,18 +139,18 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa
_interim_state.healthy = true;
}
void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const BattInfoAuxCb &cb)
void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg)
{
WITH_SEMAPHORE(_sem_battmon);
uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), cb.msg->voltage_cell.size());
float remaining_capacity_ah = _remaining_capacity_wh / cb.msg->nominal_voltage;
float full_charge_capacity_ah = _full_charge_capacity_wh / cb.msg->nominal_voltage;
uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), msg.voltage_cell.len);
float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage;
float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage;
_cycle_count = cb.msg->cycle_count;
_cycle_count = msg.cycle_count;
for (uint8_t i = 0; i < cell_count; i++) {
_interim_state.cell_voltages.cells[i] = cb.msg->voltage_cell[i] * 1000;
_interim_state.cell_voltages.cells[i] = msg.voltage_cell.data[i] * 1000;
}
_interim_state.is_powering_off = cb.msg->is_powering_off;
_interim_state.is_powering_off = msg.is_powering_off;
_interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000;
_interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh;
_interim_state.time_remaining = is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600);
@ -201,17 +162,17 @@ void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const BattInfoAuxCb &cb)
_has_battery_info_aux = true;
}
void AP_BattMonitor_UAVCAN::handle_mppt_stream(const MpptStreamCb &cb)
void AP_BattMonitor_UAVCAN::handle_mppt_stream(const mppt_Stream &msg)
{
const bool use_input_value = option_is_set(AP_BattMonitor_Params::Options::MPPT_Use_Input_Value);
const float voltage = use_input_value ? cb.msg->input_voltage : cb.msg->output_voltage;
const float current = use_input_value ? cb.msg->input_current : cb.msg->output_current;
const float voltage = use_input_value ? msg.input_voltage : msg.output_voltage;
const float current = use_input_value ? msg.input_current : msg.output_current;
// use an invalid soc so we use the library calculated one
const uint8_t soc = 127;
// convert C to Kelvin
const float temperature_K = isnan(cb.msg->temperature) ? 0 : C_TO_KELVIN(cb.msg->temperature);
const float temperature_K = isnan(msg.temperature) ? 0 : C_TO_KELVIN(msg.temperature);
update_interim_state(voltage, current, temperature_K, soc);
@ -229,38 +190,38 @@ void AP_BattMonitor_UAVCAN::handle_mppt_stream(const MpptStreamCb &cb)
}
#if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG
if (_mppt.fault_flags != cb.msg->fault_flags) {
mppt_report_faults(_instance, cb.msg->fault_flags);
if (_mppt.fault_flags != msg.fault_flags) {
mppt_report_faults(_instance, msg.fault_flags);
}
#endif
_mppt.fault_flags = cb.msg->fault_flags;
_mppt.fault_flags = msg.fault_flags;
}
void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb)
void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg)
{
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id);
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id, msg.battery_id);
if (driver == nullptr) {
return;
}
driver->handle_battery_info(cb);
driver->handle_battery_info(msg);
}
void AP_BattMonitor_UAVCAN::handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb)
void AP_BattMonitor_UAVCAN::handle_battery_info_aux_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg)
{
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id);
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id, msg.battery_id);
if (driver == nullptr) {
return;
}
driver->handle_battery_info_aux(cb);
driver->handle_battery_info_aux(msg);
}
void AP_BattMonitor_UAVCAN::handle_mppt_stream_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MpptStreamCb &cb)
void AP_BattMonitor_UAVCAN::handle_mppt_stream_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const mppt_Stream &msg)
{
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, node_id);
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id, transfer.source_node_id);
if (driver == nullptr) {
return;
}
driver->handle_mppt_stream(cb);
driver->handle_mppt_stream(msg);
}
// read - read the voltage and current
@ -351,7 +312,7 @@ void AP_BattMonitor_UAVCAN::mppt_check_powered_state()
// force should be true to force sending the state change request to the MPPT
void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on)
{
if (_ap_uavcan == nullptr || _node == nullptr || !_mppt.is_detected) {
if (_ap_uavcan == nullptr || !_mppt.is_detected) {
return;
}
@ -360,48 +321,28 @@ void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on)
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: powering %s%s", (unsigned)_instance+1, _mppt.powered_state ? "ON" : "OFF",
(_mppt.powered_state_remote_ms == 0) ? "" : " Retry");
// set up a request /w a status callback
mppt::OutputEnable::Request request;
mppt_OutputEnableRequest request;
request.enable = _mppt.powered_state;
request.disable = !request.enable;
_mppt.powered_state_remote_ms = AP_HAL::millis();
const uint8_t driver_index = _ap_uavcan->get_driver_index();
outputEnable_client[driver_index]->call(_node_id, request);
}
// callback trampoline of outputEnable response to verify it is enabled or disabled
void trampoline_handleOutputEnable(const uavcan::ServiceCallResult<mppt::OutputEnable>& resp)
{
uint8_t can_num_drivers = AP::can().get_num_drivers();
for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
if (uavcan == nullptr) {
continue;
if (mppt_outputenable_client == nullptr) {
mppt_outputenable_client = new Canard::Client<mppt_OutputEnableResponse>{_ap_uavcan->get_canard_iface(), mppt_outputenable_res_cb};
if (mppt_outputenable_client == nullptr) {
return;
}
const uint8_t node_id = resp.getResponse().getSrcNodeID().get();
AP_BattMonitor_UAVCAN* driver = AP_BattMonitor_UAVCAN::get_uavcan_backend(uavcan, node_id, node_id);
if (driver == nullptr) {
continue;
}
const auto &response = resp.getResponse();
const uint8_t nodeId = response.getSrcNodeID().get();
const bool enabled = response.enabled;
driver->handle_outputEnable_response(nodeId, enabled);
}
mppt_outputenable_client->request(_node_id, request);
}
// callback from outputEnable to verify it is enabled or disabled
void AP_BattMonitor_UAVCAN::handle_outputEnable_response(const uint8_t nodeId, const bool enabled)
void AP_BattMonitor_UAVCAN::handle_outputEnable_response(const CanardRxTransfer& transfer, const mppt_OutputEnableResponse& response)
{
if (nodeId != _node_id) {
if (transfer.source_node_id != _node_id) {
// this response is not from the node we are looking for
return;
}
if (enabled == _mppt.powered_state) {
if (response.enabled == _mppt.powered_state) {
// we got back what we expected it to be. We set it on, it now says it on (or vice versa).
// Clear the timer so we don't re-request
_mppt.powered_state_remote_ms = 0;

28
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
View File

@ -2,9 +2,8 @@
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_Backend.h"
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <mppt/OutputEnable.hpp>
#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
@ -12,10 +11,6 @@
#define AP_BATTMONITOR_UAVCAN_MPPT_DEBUG 0
#endif
class BattInfoCb;
class BattInfoAuxCb;
class MpptStreamCb;
class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
{
public:
@ -53,16 +48,15 @@ public:
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id);
static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);
static void handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb);
static void handle_mppt_stream_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MpptStreamCb &cb);
void handle_outputEnable_response(const uint8_t nodeId, const bool enabled);
static void handle_battery_info_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg);
static void handle_battery_info_aux_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg);
static void handle_mppt_stream_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const mppt_Stream &msg);
void mppt_set_powered_state(bool power_on) override;
private:
void handle_battery_info(const BattInfoCb &cb);
void handle_battery_info_aux(const BattInfoAuxCb &cb);
void handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg);
void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg);
void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc);
static bool match_battery_id(uint8_t instance, uint8_t battery_id) {
@ -77,7 +71,7 @@ private:
OVER_CURRENT = (1U<<2),
OVER_TEMPERATURE = (1U<<3),
};
void handle_mppt_stream(const MpptStreamCb &cb);
void handle_mppt_stream(const mppt_Stream &msg);
void mppt_check_powered_state();
#if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG
@ -102,7 +96,7 @@ private:
bool _has_consumed_energy;
bool _has_battery_info_aux;
uint8_t _instance; // instance of this battery monitor
uavcan::Node<0> *_node; // UAVCAN node id
AP_Float _curr_mult; // scaling multiplier applied to current reports for adjustment
// MPPT variables
struct {
@ -112,4 +106,10 @@ private:
uint8_t fault_flags; // bits holding fault flags
uint32_t powered_state_remote_ms; // timestamp of when request was sent, zeroed on response. Used to retry
} _mppt;
void handle_outputEnable_response(const CanardRxTransfer&, const mppt_OutputEnableResponse&);
Canard::ObjCallback<AP_BattMonitor_UAVCAN, mppt_OutputEnableResponse> mppt_outputenable_res_cb{this, &AP_BattMonitor_UAVCAN::handle_outputEnable_response};
Canard::Client<mppt_OutputEnableResponse> *mppt_outputenable_client;
};
#endif