Skip to content

Fix GPS heading publishing. #86

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
mdurrani808 merged 5 commits into from
Mar 29, 2026
Merged

Fix GPS heading publishing. #86

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0b07cec
fix heading publishing'
mdurrani808 Mar 17, 2026
c10caff
misread the docs on float64
mdurrani808 Mar 17, 2026
836b0ce
adhere to ros gps convention
mdurrani808 Mar 17, 2026
9a00b44
add raw compass bearing
mdurrani808 Mar 24, 2026
d945ab7
add to package.xml
mdurrani808 Mar 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Currentdraw.msg"
"msg/JointReceiver.msg"
"msg/SystemInfo.msg"
"msg/Heading.msg"
"srv/SetController.srv"
"action/NavigateToGPS.action"
DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
Expand Down
8 changes: 8 additions & 0 deletions src/msgs/msg/Heading.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
# heading: ROS convention (0=East, CCW, radians). Converted from compass bearing,
# but does not account for magnetic declination.
# compass_bearing: raw magnetometer reading (0=magnetic North, CW, degrees).

std_msgs/Header header
float64 heading
float64 heading_acc
float64 compass_bearing
2 changes: 2 additions & 0 deletions src/subsystems/navigation/athena_gps/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(msgs REQUIRED)
find_package(MAVSDK REQUIRED)
add_executable(athena_gps src/PixhawkNode.cpp)

Expand All @@ -26,6 +27,7 @@ ament_target_dependencies(athena_gps
rclcpp
std_msgs
sensor_msgs
msgs
)
target_link_libraries(athena_gps
MAVSDK::mavsdk
Expand Down
1 change: 1 addition & 0 deletions src/subsystems/navigation/athena_gps/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
69 changes: 32 additions & 37 deletions src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "std_msgs/msg/float64.hpp"
#include "msgs/msg/heading.hpp"
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/telemetry/telemetry.h>

Expand Down Expand Up @@ -54,7 +54,7 @@ class PixhawkNode : public rclcpp::Node
// Create publishers unconditionally so they're always valid
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 10);
gps_publisher_ = this->create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic, 10);
heading_publisher_ = this->create_publisher<std_msgs::msg::Float64>(heading_topic, 10);
heading_publisher_ = this->create_publisher<msgs::msg::Heading>(heading_topic, 10);

// Create MAVSDK instance
mavsdk_ = std::make_unique<Mavsdk>(Mavsdk::Configuration{
Expand All @@ -78,7 +78,7 @@ class PixhawkNode : public rclcpp::Node

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_publisher_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_publisher_;
rclcpp::Publisher<msgs::msg::Heading>::SharedPtr heading_publisher_;
rclcpp::TimerBase::SharedPtr init_timer_;

Telemetry::GpsInfo last_gps_info_;
Expand All @@ -94,12 +94,6 @@ class PixhawkNode : public rclcpp::Node
bool is_initialized_;
bool connection_added_;

void publish_heading() {
auto msg = std_msgs::msg::Float64();
msg.data = last_heading_deg_;
heading_publisher_->publish(msg);
}

void try_initialize() {
if (is_initialized_) {
return;
Expand Down Expand Up @@ -151,7 +145,6 @@ class PixhawkNode : public rclcpp::Node

RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully");

// Subscribe to telemetry streams
telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) {
imu_callback(imu);
});
Expand All @@ -160,11 +153,6 @@ class PixhawkNode : public rclcpp::Node
last_attitude_quaternion_ = quaternion;
});

telemetry_->subscribe_heading([this](Telemetry::Heading heading) {
last_heading_deg_ = heading.heading_deg;
publish_heading();
});

telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) {
raw_gps_callback(raw_gps);
});
Expand Down Expand Up @@ -228,39 +216,39 @@ class PixhawkNode : public rclcpp::Node
}

void raw_gps_callback(const Telemetry::RawGps raw_gps) {
auto msg = sensor_msgs::msg::NavSatFix();
auto gps_msg = sensor_msgs::msg::NavSatFix();

msg.header.stamp = this->now();
msg.header.frame_id = gps_frame_id_;
gps_msg.header.stamp = this->now();
gps_msg.header.frame_id = gps_frame_id_;

msg.latitude = raw_gps.latitude_deg;
msg.longitude = raw_gps.longitude_deg;
msg.altitude = raw_gps.altitude_ellipsoid_m;
gps_msg.latitude = raw_gps.latitude_deg;
gps_msg.longitude = raw_gps.longitude_deg;
gps_msg.altitude = raw_gps.altitude_ellipsoid_m;

// Map GPS fix to ROS fix
if (last_gps_info_.num_satellites > 0) {
switch (last_gps_info_.fix_type) {
case Telemetry::FixType::NoGps:
case Telemetry::FixType::NoFix:
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
break;
case Telemetry::FixType::Fix2D:
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
break;
case Telemetry::FixType::Fix3D:
case Telemetry::FixType::FixDgps:
case Telemetry::FixType::RtkFloat:
case Telemetry::FixType::RtkFixed:
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
break;
default:
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
}
} else {
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
}

msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;

// calcilate variances
if (!std::isnan(raw_gps.horizontal_uncertainty_m) &&
Expand All @@ -269,23 +257,30 @@ class PixhawkNode : public rclcpp::Node
double h_variance = raw_gps.horizontal_uncertainty_m * raw_gps.horizontal_uncertainty_m;
double v_variance = raw_gps.vertical_uncertainty_m * raw_gps.vertical_uncertainty_m;

msg.position_covariance[0] = h_variance;
msg.position_covariance[4] = h_variance;
msg.position_covariance[8] = v_variance;
gps_msg.position_covariance[0] = h_variance;
gps_msg.position_covariance[4] = h_variance;
gps_msg.position_covariance[8] = v_variance;

// refine estimate using doppler
if (!std::isnan(raw_gps.hdop) && !std::isnan(raw_gps.vdop)) {
msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
msg.position_covariance[8] = v_variance * raw_gps.vdop;
gps_msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
gps_msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
gps_msg.position_covariance[8] = v_variance * raw_gps.vdop;
}

msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
} else {
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}

gps_publisher_->publish(msg);
auto heading_msg = msgs::msg::Heading();
heading_msg.header.stamp = this->now();
heading_msg.compass_bearing = raw_gps.yaw_deg;
// Convert from compass bearing (0=magnetic North, CW, degrees) to ROS convention (0=East, CCW, radians)
heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0;
heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0;

heading_publisher_->publish(heading_msg);
gps_publisher_->publish(gps_msg);
}
};

Expand Down
Loading