diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 4896a1e81a..db635a5395 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp @@ -171,29 +171,29 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node { WITH_SEMAPHORE(_sem_registry); - Vector3f mag_vector; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); - if (driver != nullptr) { - mag_vector[0] = cb.msg->magnetic_field_ga[0]; - mag_vector[1] = cb.msg->magnetic_field_ga[1]; - mag_vector[2] = cb.msg->magnetic_field_ga[2]; - driver->handle_mag_msg(mag_vector); - } + Vector3f mag_vector; + AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); + if (driver != nullptr) { + mag_vector[0] = cb.msg->magnetic_field_ga[0]; + mag_vector[1] = cb.msg->magnetic_field_ga[1]; + mag_vector[2] = cb.msg->magnetic_field_ga[2]; + driver->handle_mag_msg(mag_vector); + } } void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb) { WITH_SEMAPHORE(_sem_registry); - Vector3f mag_vector; - uint8_t sensor_id = cb.msg->sensor_id; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); - if (driver != nullptr) { - mag_vector[0] = cb.msg->magnetic_field_ga[0]; - mag_vector[1] = cb.msg->magnetic_field_ga[1]; - mag_vector[2] = cb.msg->magnetic_field_ga[2]; - driver->handle_mag_msg(mag_vector); - } + Vector3f mag_vector; + uint8_t sensor_id = cb.msg->sensor_id; + AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); + if (driver != nullptr) { + mag_vector[0] = cb.msg->magnetic_field_ga[0]; + mag_vector[1] = cb.msg->magnetic_field_ga[1]; + mag_vector[2] = cb.msg->magnetic_field_ga[2]; + driver->handle_mag_msg(mag_vector); + } } void AP_Compass_UAVCAN::read(void)