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

AP_Compass: 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 9f7ed65bc0
commit d86bab9c58
2 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Compass/AP_Compass_DroneCAN.cpp
View File

@ -37,24 +37,16 @@ AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devi
{ {
} }
void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) bool AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
{ {
if (ap_dronecan == nullptr) { const auto driver_index = ap_dronecan->get_driver_index();
return;
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("mag_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("mag2_sub");
}
return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, driver_index) != nullptr)
&& (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, driver_index) != nullptr)
#if AP_COMPASS_DRONECAN_HIRES_ENABLED #if AP_COMPASS_DRONECAN_HIRES_ENABLED
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, ap_dronecan->get_driver_index()) == nullptr) { && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, driver_index) != nullptr)
AP_BoardConfig::allocation_error("mag3_sub");
}
#endif #endif
;
} }
AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index) AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index)

2
libraries/AP_Compass/AP_Compass_DroneCAN.h
View File

@ -14,7 +14,7 @@ public:
void read(void) override; void read(void) override;
static void subscribe_msgs(AP_DroneCAN* ap_dronecan); static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
static AP_Compass_Backend* probe(uint8_t index); static AP_Compass_Backend* probe(uint8_t index);
static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; } static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg); static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);