mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: fix UAVCAN mag message handlers
This commit is contained in:
parent
bcaa4901ed
commit
2c0b9a16a5
|
@ -205,13 +205,12 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node
|
|||
if (take_registry()) {
|
||||
Vector3f mag_vector;
|
||||
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0);
|
||||
if (driver == nullptr) {
|
||||
return;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
give_registry();
|
||||
}
|
||||
}
|
||||
|
@ -222,13 +221,12 @@ void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t no
|
|||
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) {
|
||||
return;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
give_registry();
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue