diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index d3340a2528..d635d9579d 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -139,6 +139,8 @@ private: if (response.isResponseEnabled()) { + publisher_.setPriority(request.getPriority()); // Responding at the same priority. + const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), request.getTransferID()); if (res < 0) diff --git a/libuavcan/test/node/service_server.cpp b/libuavcan/test/node/service_server.cpp index 4aa56572b3..3d1efcbe9b 100644 --- a/libuavcan/test/node/service_server.cpp +++ b/libuavcan/test/node/service_server.cpp @@ -84,7 +84,7 @@ TEST(ServiceServer, Basic) frame.setStartOfTransfer(true); frame.setEndOfTransfer(true); - frame.setPriority(10); + frame.setPriority(i); uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); can_driver.ifaces[0].pushRx(rx_frame); @@ -111,6 +111,7 @@ TEST(ServiceServer, Basic) ASSERT_EQ(i, fr.getTransferID().get()); ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType()); ASSERT_EQ(i + 0x10, fr.getDstNodeID().get()); + ASSERT_EQ(i, fr.getPriority().get()); // Second frame ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame()));