diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 19364bd969..f2c9e48c50 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1452,7 +1452,7 @@ void Compass::_detect_backends(void) #if AP_COMPASS_DRONECAN_ENABLED if (_driver_enabled(DRIVER_UAVCAN)) { for (uint8_t i=0; iinit()) { delete driver; @@ -94,7 +94,7 @@ AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index) return driver; } -bool AP_Compass_UAVCAN::init() +bool AP_Compass_DroneCAN::init() { // Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default if (!register_compass(_devid, _instance)) { @@ -104,11 +104,11 @@ bool AP_Compass_UAVCAN::init() set_dev_id(_instance, _devid); set_external(_instance, true); - AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_UAVCAN loaded\n\r"); + AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_DroneCAN loaded\n\r"); return true; } -AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id) +AP_Compass_DroneCAN* AP_Compass_DroneCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id) { if (ap_dronecan == nullptr) { return nullptr; @@ -165,19 +165,19 @@ AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_droneca return nullptr; } -void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag) +void AP_Compass_DroneCAN::handle_mag_msg(const Vector3f &mag) { Vector3f raw_field = mag * 1000.0; accumulate_sample(raw_field, _instance); } -void AP_Compass_UAVCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg) +void AP_Compass_DroneCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg) { WITH_SEMAPHORE(_sem_registry); Vector3f mag_vector; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, 0); + AP_Compass_DroneCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, 0); if (driver != nullptr) { mag_vector[0] = msg.magnetic_field_ga[0]; mag_vector[1] = msg.magnetic_field_ga[1]; @@ -186,13 +186,13 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const Ca } } -void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg) +void AP_Compass_DroneCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg) { WITH_SEMAPHORE(_sem_registry); Vector3f mag_vector; uint8_t sensor_id = msg.sensor_id; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, sensor_id); + AP_Compass_DroneCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, sensor_id); if (driver != nullptr) { mag_vector[0] = msg.magnetic_field_ga[0]; mag_vector[1] = msg.magnetic_field_ga[1]; @@ -201,7 +201,7 @@ void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const } } -void AP_Compass_UAVCAN::read(void) +void AP_Compass_DroneCAN::read(void) { drain_accumulated_samples(_instance); } diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.h b/libraries/AP_Compass/AP_Compass_DroneCAN.h index e068fb4f2f..45edf38537 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.h +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.h @@ -8,9 +8,9 @@ #include -class AP_Compass_UAVCAN : public AP_Compass_Backend { +class AP_Compass_DroneCAN : public AP_Compass_Backend { public: - AP_Compass_UAVCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid); + AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid); void read(void) override; @@ -26,7 +26,7 @@ private: // callback for DroneCAN messages void handle_mag_msg(const Vector3f &mag); - static AP_Compass_UAVCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id); + static AP_Compass_DroneCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id); uint8_t _instance; @@ -40,7 +40,7 @@ private: AP_DroneCAN* ap_dronecan; uint8_t node_id; uint8_t sensor_id; - AP_Compass_UAVCAN *driver; + AP_Compass_DroneCAN *driver; uint32_t devid; } _detected_modules[COMPASS_MAX_BACKEND]; diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index 830b84c81e..72ae8a411c 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -26,7 +26,7 @@ #endif #ifndef AP_COMPASS_DRONECAN_ENABLED -#define AP_COMPASS_DRONECAN_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS) +#define AP_COMPASS_DRONECAN_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS) #endif // i2c-based compasses: