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

AP_Compass: fix UAVCAN mag message handlers

This commit is contained in:
liang.tang 2018-09-04 21:41:57 -07:00 committed by Andrew Tridgell
parent bcaa4901ed
commit 2c0b9a16a5
1 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -205,13 +205,12 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node
if (take_registry()) { if (take_registry()) {
Vector3f mag_vector; Vector3f mag_vector;
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0);
if (driver == nullptr) { if (driver != nullptr) {
return; 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(); give_registry();
} }
} }
@ -222,13 +221,12 @@ void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t no
Vector3f mag_vector; Vector3f mag_vector;
uint8_t sensor_id = cb.msg->sensor_id; uint8_t sensor_id = cb.msg->sensor_id;
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id);
if (driver == nullptr) { if (driver != nullptr) {
return; 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(); give_registry();
} }
} }