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

AP_GPS: 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 8328368164
commit 360e54f871
2 changed files with 10 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -89,36 +89,19 @@ AP_GPS_DroneCAN::~AP_GPS_DroneCAN()
#endif
}
void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
bool AP_GPS_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_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub");
}
return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, driver_index) != nullptr)
#if GPS_MOVING_BASELINE
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("moving_baseline_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("relposheading_sub");
}
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, driver_index) != nullptr)
#endif
;
}
AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)

2
libraries/AP_GPS/AP_GPS_DroneCAN.h
View File

@ -44,7 +44,7 @@ public:
const char *name() const override { return _name; }
static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg);