Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: optimize DroneCAN subscription process 

* remove unnecessary nullptr check, these are always called from an
  initialized AP_DroneCAN so if it's nullptr something has gone
  horrifically wrong

* pass in driver index instead of repeatedly calling function to get it

* simplify error handling; knowing exactly which allocation failed is not
  super helpful and one failing likely means subsequent ones will too,
  as it can only fail due to being out of memory
This commit is contained in:
Thomas Watson 2024-11-16 21:21:13 -06:00 committed by Andrew Tridgell
parent def199e61f
commit 9f7ed65bc0
2 changed files with 7 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
View File

@ -42,23 +42,14 @@ AP_BattMonitor_DroneCAN::AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMon
_state.healthy = false;
}
void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
bool AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
{
if (ap_dronecan == nullptr) {
return;
}
const auto driver_index = ap_dronecan->get_driver_index();
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("battinfo_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("battinfo_aux_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("mppt_stream_sub");
}
return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, driver_index) != nullptr)
;
}
/*

2
libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h
View File

@ -47,7 +47,7 @@ public:
// return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
uint32_t get_mavlink_fault_bitmask() const override;
static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
static AP_BattMonitor_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id);
static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg);
static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg);