From 8fd3e3aa9db412087531c5fa4bcfc07dc3b19c4c Mon Sep 17 00:00:00 2001 From: Alon Nusem Date: Wed, 7 May 2025 13:39:04 +1000 Subject: [PATCH 1/2] Replaced quaternion standard deviation with euler covariance Original decoder used quaternion standard deviation elements Q0, Q1, and Q2 directly as covariances around the x, y, and z axes in the IMU message. This doesn't seem like a valid way to interpret these values. Euler standard deviation converted into covariance has replaced this to give better values in the IMU message covariance matrix. --- .../include/adnav_driver/adnav_driver.h | 4 ++-- adnav_driver/src/adnav_driver.cpp | 20 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/adnav_driver/include/adnav_driver/adnav_driver.h b/adnav_driver/include/adnav_driver/adnav_driver.h index 9855254..89f60b4 100644 --- a/adnav_driver/include/adnav_driver/adnav_driver.h +++ b/adnav_driver/include/adnav_driver/adnav_driver.h @@ -133,7 +133,7 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin private: // Debug variables - int pub_num_ = 0, P28_num_ = 0, P20_num_ = 0, P27_num_ = 0, P33_num_ = 0, P0_num_ = 0; + int pub_num_ = 0, P28_num_ = 0, P20_num_ = 0, P26_num_ = 0, P33_num_ = 0, P0_num_ = 0; // Defines what communication method to use, refer to adnav_driver_connection_e. int communication_state_; @@ -297,7 +297,7 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin void deviceInfoDecoder(an_packet_t* an_packet); void systemStateRosDecoder(an_packet_t* an_packet); void ecefPosRosDecoder(an_packet_t* an_packet); - void quartOrientSDRosDriver(an_packet_t* an_packet); + void eulerOrientSDRosDriver(an_packet_t* an_packet); void rawSensorsRosDecoder(an_packet_t* an_packet); }; diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 7833959..7bf4ae0 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -1473,7 +1473,7 @@ void Driver::decodePackets(an_decoder_t &an_decoder, const int &bytes) { case packet_id_ecef_position: ecefPosRosDecoder(an_packet); break; - case packet_id_quaternion_orientation_standard_deviation: quartOrientSDRosDriver(an_packet); + case packet_id_euler_orientation_standard_deviation: eulerOrientSDRosDriver(an_packet); break; case packet_id_raw_sensors: rawSensorsRosDecoder(an_packet); @@ -1833,33 +1833,33 @@ void Driver::ecefPosRosDecoder(an_packet_t* an_packet) { } /** - * @brief Function to decode the Quaternion Orientation Standard Deviation ANPP Packet (ANPP.27). + * @brief Function to decode the Euler Orientation Standard Deviation ANPP Packet (ANPP.26). * * This function accesses in a thread safe manner the class stored ROS messages, placed relevant information into them, * then using the publishing control variable, requests a publisher thread to publish the message. * * @param an_packet a pointer to an an_packet_t object which will be decoded. */ -void Driver::quartOrientSDRosDriver(an_packet_t* an_packet) { - quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; +void Driver::eulerOrientSDRosDriver(an_packet_t* an_packet) { + euler_orientation_standard_deviation_packet_t euler_orientation_standard_deviation_packet; std::unique_lock lock(messages_mutex_); - RCLCPP_DEBUG(this->get_logger(), "Packet 27: \tMutex: L\tAccess: %d", P27_num_); + RCLCPP_DEBUG(this->get_logger(), "Packet 26: \tMutex: L\tAccess: %d", P26_num_); // Debug timekeeper auto time = this->get_clock().get()->now().nanoseconds(); - if(decode_quaternion_orientation_standard_deviation_packet(&quaternion_orientation_standard_deviation_packet, an_packet) == 0) + if(decode_euler_orientation_standard_deviation_packet(&euler_orientation_standard_deviation_packet, an_packet) == 0) { // IMU message - imu_msg_.orientation_covariance[0] = quaternion_orientation_standard_deviation_packet.standard_deviation[0]; - imu_msg_.orientation_covariance[4] = quaternion_orientation_standard_deviation_packet.standard_deviation[1]; - imu_msg_.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; + imu_msg_.orientation_covariance[0] = pow(euler_orientation_standard_deviation_packet.standard_deviation[0], 2); + imu_msg_.orientation_covariance[4] = pow(euler_orientation_standard_deviation_packet.standard_deviation[1], 2); + imu_msg_.orientation_covariance[8] = pow(euler_orientation_standard_deviation_packet.standard_deviation[2], 2); } // Now that work is complete notify an update for the publisher. msg_write_done_ = true; msg_cv_.notify_one(); // RCLCPP_DEBUG(this->get_logger(), "Raw: \tNotifying Complete\t%d", raw_num_++); auto diff = this->get_clock().get()->now().nanoseconds() - time; - RCLCPP_DEBUG(this->get_logger(), "Packet 27:\tMutex: U\tAccess: %d\tTimeLocked: %ld μs", P27_num_++, diff/1000); + RCLCPP_DEBUG(this->get_logger(), "Packet 26:\tMutex: U\tAccess: %d\tTimeLocked: %ld μs", P26_num_++, diff/1000); } /** From 8533886c4b121d8fdbdce9813ae3c92a429229c6 Mon Sep 17 00:00:00 2001 From: Alon Nusem Date: Wed, 7 May 2025 14:25:02 +1000 Subject: [PATCH 2/2] Updated readme to include euler standard deviation supported message --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 5b5e05d..775b20c 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ This example is currently able to decode the following packets into ROS messages - ANPP 0: Acknowledge Packet - ANPP 3: Device Information Packet - ANPP 20: System State Packet +- ANPP 26: Euler Orientation Standard Deviation Packet - ANPP 28: Raw Sensors Packet - ANPP 33: ECEF Position Packet @@ -455,4 +456,4 @@ And in the Drivers terminal ``` NtripClient service done. [adnav_node]: RTCM Log file for this session can be found in: Log____