|
|
|
@ -171,29 +171,29 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node
@@ -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) |
|
|
|
|