From 2c0b9a16a5697092493c7076563de4df9e61815e Mon Sep 17 00:00:00 2001 From: "liang.tang" <466175335@qq.com> Date: Tue, 4 Sep 2018 21:41:57 -0700 Subject: [PATCH] AP_Compass: fix UAVCAN mag message handlers --- libraries/AP_Compass/AP_Compass_UAVCAN.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 66ddf3e0f4..b785cd7a16 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp @@ -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(); } }