Skip to content
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

Add satellite status msgs #71

Draft
wants to merge 9 commits into
base: main
Choose a base branch
from
Draft

Add satellite status msgs #71

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
9f93ea4
add satellite status msg
Aposhian Apr 26, 2024
c0b2104
enable satellite status stream
Aposhian Apr 26, 2024
d75de14
Activate publishers
Aposhian Apr 26, 2024
26aea9c
Configure publishers
Aposhian Apr 27, 2024
42947f0
Remove satellite status epoch summary
Aposhian Apr 30, 2024
d74f6ad
Add mip gnss raw support
Aposhian Apr 30, 2024
52ffcb3
fixup! Add mip gnss raw support
Aposhian Apr 30, 2024
11edb59
fixup! Add mip gnss raw support
Aposhian Apr 30, 2024
63b92a2
Merge branch 'main-firefly' into add-satellite-status-msgs
Aposhian Jun 14, 2024
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
4 changes: 4 additions & 0 deletions config/params.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -457,10 +457,14 @@ mip_sensor_overrange_status_data_rate : 0 # Rate of mip/sensor/overrange_status
mip_gnss1_fix_info_data_rate : 0 # Rate of mip/gnss_1/fix_info topic
mip_gnss1_sbas_info_data_rate : 0 # Rate of mip/gnss_1/sbas_info topic
mip_gnss1_rf_error_detection_data_rate : 0 # Rate of mip/gnss_1/rf_error_detection
mip_gnss1_satellite_status_data_rate : 0 # Rate of mip/gnss_1/satellite_status
mip_gnss1_raw_data_rate : 0 # Rate of mip/gnss_1/raw

mip_gnss2_fix_info_data_rate : 0 # Rate of mip/gnss_1/fix_info topic
mip_gnss2_sbas_info_data_rate : 0 # Rate of mip/gnss_2/sbas_info topic
mip_gnss2_rf_error_detection_data_rate : 0 # Rate of mip/gnss_2/rf_error_detection
mip_gnss2_satellite_status_data_rate : 0 # Rate of mip/gnss_2/satellite_status
mip_gnss2_raw_data_rate : 0 # Rate of mip/gnss_2/raw

mip_filter_status_data_rate : 0 # Rate of mip/filter/status topic
mip_filter_gnss_position_aiding_status_data_rate : 0 # Rate of mip/filter/gnss_aiding_status
Expand Down
4 changes: 4 additions & 0 deletions include/microstrain_inertial_driver_common/publishers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,8 @@ class Publishers
Publisher<MipGnssFixInfoMsg>::SharedPtrVec mip_gnss_fix_info_pub_ = Publisher<MipGnssFixInfoMsg>::initializeVec({MIP_GNSS1_FIX_INFO_TOPIC, MIP_GNSS2_FIX_INFO_TOPIC});
Publisher<MipGnssSbasInfoMsg>::SharedPtrVec mip_gnss_sbas_info_pub_ = Publisher<MipGnssSbasInfoMsg>::initializeVec({MIP_GNSS1_SBAS_INFO_TOPIC, MIP_GNSS2_SBAS_INFO_TOPIC});
Publisher<MipGnssRfErrorDetectionMsg>::SharedPtrVec mip_gnss_rf_error_detection_pub_ = Publisher<MipGnssRfErrorDetectionMsg>::initializeVec({MIP_GNSS1_RF_ERROR_DETECTION_TOPIC, MIP_GNSS2_RF_ERROR_DETECTION_TOPIC});
Publisher<MipGnssSatelliteStatusMsg>::SharedPtrVec mip_gnss_satellite_status_pub_ = Publisher<MipGnssSatelliteStatusMsg>::initializeVec({MIP_GNSS1_SATELLITE_STATUS_TOPIC, MIP_GNSS2_SATELLITE_STATUS_TOPIC});
Publisher<MipGnssRawMsg>::SharedPtrVec mip_gnss_raw_pub_ = Publisher<MipGnssRawMsg>::initializeVec({MIP_GNSS1_RAW_TOPIC, MIP_GNSS2_RAW_TOPIC});

// MIP GNSS Corrections (0x93) publishers
Publisher<MipGnssCorrectionsRtkCorrectionsStatusMsg>::SharedPtr mip_gnss_corrections_rtk_corrections_status_pub_ = Publisher<MipGnssCorrectionsRtkCorrectionsStatusMsg>::initialize(MIP_GNSS_CORRECTIONS_RTK_CORRECTIONS_STATUS_TOPIC);
Expand Down Expand Up @@ -343,6 +345,8 @@ class Publishers
void handleGnssFixInfo(const mip::data_gnss::FixInfo& fix_info, const uint8_t descriptor_set, mip::Timestamp timestamp);
void handleGnssSbasInfo(const mip::data_gnss::SbasInfo& sbas_info, const uint8_t descriptor_set, mip::Timestamp timestamp);
void handleGnssRfErrorDetection(const mip::data_gnss::RfErrorDetection& rf_error_detection, const uint8_t descriptor_set, mip::Timestamp timestamp);
void handleGnssSatelliteStatus(const mip::data_gnss::SatelliteStatus& satellite_status, const uint8_t descriptor_set, mip::Timestamp timestamp);
void handleGnssRaw(const mip::data_gnss::Raw& raw, const uint8_t descriptor_set, mip::Timestamp timestamp);

// Callbacks to handle RTK data from the device
void handleRtkCorrectionsStatus(const mip::data_gnss::RtkCorrectionsStatus& rtk_corrections_status, const uint8_t descriptor_set, mip::Timestamp timestamp);
Expand Down
4 changes: 4 additions & 0 deletions include/microstrain_inertial_driver_common/utils/mappings/mip_publisher_mapping.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,14 @@ static constexpr auto MIP_SENSOR_OVERRANGE_STATUS_TOPIC = "mip/sensor/overrange_
static constexpr auto MIP_GNSS1_FIX_INFO_TOPIC = "mip/gnss_1/fix_info";
static constexpr auto MIP_GNSS1_SBAS_INFO_TOPIC = "mip/gnss_1/sbas_info";
static constexpr auto MIP_GNSS1_RF_ERROR_DETECTION_TOPIC = "mip/gnss_1/rf_error_detection";
static constexpr auto MIP_GNSS1_SATELLITE_STATUS_TOPIC = "mip/gnss_1/satellite_status";
static constexpr auto MIP_GNSS1_RAW_TOPIC = "mip/gnss_1/raw";

static constexpr auto MIP_GNSS2_FIX_INFO_TOPIC = "mip/gnss_2/fix_info";
static constexpr auto MIP_GNSS2_SBAS_INFO_TOPIC = "mip/gnss_2/sbas_info";
static constexpr auto MIP_GNSS2_RF_ERROR_DETECTION_TOPIC = "mip/gnss_2/rf_error_detection";
static constexpr auto MIP_GNSS2_SATELLITE_STATUS_TOPIC = "mip/gnss_2/satellite_status";
static constexpr auto MIP_GNSS2_RAW_TOPIC = "mip/gnss_2/raw";

static constexpr auto MIP_GNSS_CORRECTIONS_RTK_CORRECTIONS_STATUS_TOPIC = "mip/gnss_corrections/rtk_corrections_status";

Expand Down
8 changes: 8 additions & 0 deletions include/microstrain_inertial_driver_common/utils/ros_compat.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,8 @@ constexpr auto NUM_GNSS = 2;
#include "microstrain_inertial_msgs/MipGnssFixInfo.h"
#include "microstrain_inertial_msgs/MipGnssSbasInfo.h"
#include "microstrain_inertial_msgs/MipGnssRfErrorDetection.h"
#include "microstrain_inertial_msgs/MipGnssSatelliteStatus.h"
#include "microstrain_inertial_msgs/MipGnssRaw.h"
#include "microstrain_inertial_msgs/MipGnssCorrectionsRtkCorrectionsStatus.h"
#include "microstrain_inertial_msgs/MipFilterStatus.h"
#include "microstrain_inertial_msgs/MipFilterGnssPositionAidingStatus.h"
Expand Down Expand Up @@ -139,6 +141,8 @@ constexpr auto NUM_GNSS = 2;
#include "microstrain_inertial_msgs/msg/mip_header.hpp"
#include "microstrain_inertial_msgs/msg/mip_sensor_overrange_status.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_fix_info.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_satellite_status.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_raw.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_sbas_info.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_rf_error_detection.hpp"
#include "microstrain_inertial_msgs/msg/mip_gnss_corrections_rtk_corrections_status.hpp"
Expand Down Expand Up @@ -249,6 +253,8 @@ using MipSensorOverrangeStatusMsg = ::microstrain_inertial_msgs::MipSensorOverra
using MipGnssFixInfoMsg = ::microstrain_inertial_msgs::MipGnssFixInfo;
using MipGnssSbasInfoMsg = ::microstrain_inertial_msgs::MipGnssSbasInfo;
using MipGnssRfErrorDetectionMsg = ::microstrain_inertial_msgs::MipGnssRfErrorDetection;
using MipGnssSatelliteStatusMsg = ::microstrain_inertial_msgs::MipGnssSatelliteStatus;
using MipGnssRawMsg = ::microstrain_inertial_msgs::MipGnssRaw;
using MipGnssCorrectionsRtkCorrectionsStatusMsg = ::microstrain_inertial_msgs::MipGnssCorrectionsRtkCorrectionsStatus;
using MipFilterGnssPositionAidingStatusMsg = ::microstrain_inertial_msgs::MipFilterGnssPositionAidingStatus;
using MipFilterStatusMsg = ::microstrain_inertial_msgs::MipFilterStatus;
Expand Down Expand Up @@ -553,6 +559,8 @@ using MipSensorOverrangeStatusMsg = ::microstrain_inertial_msgs::msg::MipSensorO
using MipGnssFixInfoMsg = ::microstrain_inertial_msgs::msg::MipGnssFixInfo;
using MipGnssSbasInfoMsg = ::microstrain_inertial_msgs::msg::MipGnssSbasInfo;
using MipGnssRfErrorDetectionMsg = ::microstrain_inertial_msgs::msg::MipGnssRfErrorDetection;
using MipGnssSatelliteStatusMsg = ::microstrain_inertial_msgs::msg::MipGnssSatelliteStatus;
using MipGnssRawMsg = ::microstrain_inertial_msgs::msg::MipGnssRaw;
using MipGnssCorrectionsRtkCorrectionsStatusMsg = ::microstrain_inertial_msgs::msg::MipGnssCorrectionsRtkCorrectionsStatus;
using MipFilterStatusMsg = ::microstrain_inertial_msgs::msg::MipFilterStatus;
using MipFilterGnssPositionAidingStatusMsg = ::microstrain_inertial_msgs::msg::MipFilterGnssPositionAidingStatus;
Expand Down
101 changes: 101 additions & 0 deletions src/publishers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,8 @@ bool Publishers::configure()
for (const auto& pub : mip_gnss_fix_info_pub_) pub->configure(node_, config_);
for (const auto& pub : mip_gnss_sbas_info_pub_) pub->configure(node_, config_);
for (const auto& pub : mip_gnss_rf_error_detection_pub_) pub->configure(node_, config_);
for (const auto& pub : mip_gnss_satellite_status_pub_) pub->configure(node_, config_);
for (const auto& pub : mip_gnss_raw_pub_) pub->configure(node_, config_);

if (config_->rtk_dongle_enable_ && config_->mip_device_->supportsDescriptor(mip::data_gnss::MIP_GNSS3_DATA_DESC_SET, mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS))
mip_gnss_corrections_rtk_corrections_status_pub_->configure(node_);
Expand Down Expand Up @@ -341,6 +343,8 @@ bool Publishers::configure()
registerDataCallback<mip::data_gnss::FixInfo, &Publishers::handleGnssFixInfo>(gnss_descriptor_set);
registerDataCallback<mip::data_gnss::SbasInfo, &Publishers::handleGnssSbasInfo>(gnss_descriptor_set);
registerDataCallback<mip::data_gnss::RfErrorDetection, &Publishers::handleGnssRfErrorDetection>(gnss_descriptor_set);
registerDataCallback<mip::data_gnss::SatelliteStatus, &Publishers::handleGnssSatelliteStatus>(gnss_descriptor_set);
registerDataCallback<mip::data_gnss::Raw, &Publishers::handleGnssRaw>(gnss_descriptor_set);
}

// Note: It is important to make sure this is after the GNSS1/2 callbacks
Expand Down Expand Up @@ -405,6 +409,8 @@ bool Publishers::activate()
for (const auto& pub : mip_gnss_fix_info_pub_) pub->activate();
for (const auto& pub : mip_gnss_sbas_info_pub_) pub->activate();
for (const auto& pub : mip_gnss_rf_error_detection_pub_) pub->activate();
for (const auto& pub : mip_gnss_satellite_status_pub_) pub->activate();
for (const auto& pub : mip_gnss_raw_pub_) pub->activate();

mip_gnss_corrections_rtk_corrections_status_pub_->activate();

Expand Down Expand Up @@ -460,6 +466,8 @@ bool Publishers::deactivate()
for (const auto& pub : mip_gnss_fix_info_pub_) pub->deactivate();
for (const auto& pub : mip_gnss_sbas_info_pub_) pub->deactivate();
for (const auto& pub : mip_gnss_rf_error_detection_pub_) pub->deactivate();
for (const auto& pub : mip_gnss_satellite_status_pub_) pub->deactivate();
for (const auto& pub : mip_gnss_raw_pub_) pub->deactivate();

mip_gnss_corrections_rtk_corrections_status_pub_->deactivate();

Expand Down Expand Up @@ -980,6 +988,99 @@ void Publishers::handleGnssSbasInfo(const mip::data_gnss::SbasInfo& sbas_info, c
mip_gnss_sbas_info_pub_[gnss_index]->publish(*mip_gnss_sbas_info_msg);
}

void Publishers::handleGnssSatelliteStatus(const mip::data_gnss::SatelliteStatus& satellite_status, const uint8_t descriptor_set, mip::Timestamp timestamp)
{
// Find the right index for the message
uint8_t gnss_index;
switch (descriptor_set)
{
case mip::data_gnss::MIP_GNSS1_DATA_DESC_SET:
gnss_index = 0;
break;
case mip::data_gnss::MIP_GNSS2_DATA_DESC_SET:
gnss_index = 1;
break;
default:
return; // Nothing to do if the descriptor set is not something we recognize
}

// Different message depending on descriptor
auto mip_gnss_satellite_status_msg = mip_gnss_satellite_status_pub_[gnss_index]->getMessage();
updateMipHeader(&(mip_gnss_satellite_status_msg->header), descriptor_set);

mip_gnss_satellite_status_msg->gnss_id = static_cast<uint8_t>(satellite_status.gnss_id);
mip_gnss_satellite_status_msg->satellite_id = static_cast<uint8_t>(satellite_status.satellite_id);
mip_gnss_satellite_status_msg->elevation = satellite_status.elevation;
mip_gnss_satellite_status_msg->azimuth = satellite_status.azimuth;
mip_gnss_satellite_status_msg->health = satellite_status.health;

mip_gnss_satellite_status_msg->valid_flags.tow = satellite_status.valid_flags.tow();
mip_gnss_satellite_status_msg->valid_flags.week_number = satellite_status.valid_flags.weekNumber();
mip_gnss_satellite_status_msg->valid_flags.gnss_id = satellite_status.valid_flags.gnssId();
mip_gnss_satellite_status_msg->valid_flags.satellite_id = satellite_status.valid_flags.satelliteId();
mip_gnss_satellite_status_msg->valid_flags.elevation = satellite_status.valid_flags.elevation();
mip_gnss_satellite_status_msg->valid_flags.azimuth = satellite_status.valid_flags.azimuth();
mip_gnss_satellite_status_msg->valid_flags.health = satellite_status.valid_flags.health();

mip_gnss_satellite_status_pub_[gnss_index]->publish(*mip_gnss_satellite_status_msg);
}

void Publishers::handleGnssRaw(const mip::data_gnss::Raw& raw, const uint8_t descriptor_set, mip::Timestamp timestamp)
{
// Find the right index for the message
uint8_t gnss_index;
switch (descriptor_set)
{
case mip::data_gnss::MIP_GNSS1_DATA_DESC_SET:
gnss_index = 0;
break;
case mip::data_gnss::MIP_GNSS2_DATA_DESC_SET:
gnss_index = 1;
break;
default:
return; // Nothing to do if the descriptor set is not something we recognize
}

// Different message depending on descriptor
auto mip_gnss_raw_msg = mip_gnss_raw_pub_[gnss_index]->getMessage();
updateMipHeader(&(mip_gnss_raw_msg->header), descriptor_set);

mip_gnss_raw_msg->index = raw.index;
mip_gnss_raw_msg->count = raw.count;
mip_gnss_raw_msg->receiver_id = raw.receiver_id;
mip_gnss_raw_msg->tracking_channel = raw.tracking_channel;
mip_gnss_raw_msg->gnss_id = static_cast<uint8_t>(raw.gnss_id);
mip_gnss_raw_msg->satellite_id = raw.satellite_id;
mip_gnss_raw_msg->signal_id = static_cast<uint8_t>(raw.signal_id);
mip_gnss_raw_msg->signal_strength = raw.signal_strength;
mip_gnss_raw_msg->quality = static_cast<uint8_t>(raw.quality);
mip_gnss_raw_msg->pseudorange = raw.carrier_phase;
mip_gnss_raw_msg->doppler = raw.doppler;
mip_gnss_raw_msg->range_uncertainty = raw.range_uncert;
mip_gnss_raw_msg->phase_uncertainty = raw.phase_uncert;
mip_gnss_raw_msg->doppler_uncertainty = raw.doppler_uncert;
mip_gnss_raw_msg->lock_time = raw.lock_time;

mip_gnss_raw_msg->valid_flags.tow = raw.valid_flags.tow();
mip_gnss_raw_msg->valid_flags.week_number = raw.valid_flags.weekNumber();
mip_gnss_raw_msg->valid_flags.receiver_id = raw.valid_flags.receiverId();
mip_gnss_raw_msg->valid_flags.tracking_channel = raw.valid_flags.trackingChannel();
mip_gnss_raw_msg->valid_flags.gnss_id = raw.valid_flags.gnssId();
mip_gnss_raw_msg->valid_flags.satellite_id = raw.valid_flags.satelliteId();
mip_gnss_raw_msg->valid_flags.signal_id = raw.valid_flags.signalId();
mip_gnss_raw_msg->valid_flags.signal_strength = raw.valid_flags.signalStrength();
mip_gnss_raw_msg->valid_flags.quality = raw.valid_flags.quality();
mip_gnss_raw_msg->valid_flags.pseudorange = raw.valid_flags.pseudorange();
mip_gnss_raw_msg->valid_flags.carrier_phase = raw.valid_flags.carrierPhase();
mip_gnss_raw_msg->valid_flags.doppler = raw.valid_flags.doppler();
mip_gnss_raw_msg->valid_flags.range_uncertainty = raw.valid_flags.rangeUncertainty();
mip_gnss_raw_msg->valid_flags.carrier_phase_uncertainty = raw.valid_flags.carrierPhaseUncertainty();
mip_gnss_raw_msg->valid_flags.doppler_uncertainty = raw.valid_flags.dopplerUncertainty();
mip_gnss_raw_msg->valid_flags.lock_time = raw.valid_flags.lockTime();

mip_gnss_raw_pub_[gnss_index]->publish(*mip_gnss_raw_msg);
}

void Publishers::handleRtkCorrectionsStatus(const mip::data_gnss::RtkCorrectionsStatus& rtk_corrections_status, const uint8_t descriptor_set, mip::Timestamp timestamp)
{
const mip::commands_rtk::GetStatusFlags::StatusFlags dongle_status(rtk_corrections_status.dongle_status);
Expand Down
16 changes: 16 additions & 0 deletions src/utils/mappings/mip_publisher_mapping.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -397,6 +397,12 @@ const std::map<std::string, FieldWrapper::SharedPtrVec> MipPublisherMapping::sta
{MIP_GNSS1_RF_ERROR_DETECTION_TOPIC, {
FieldWrapperType<mip::data_gnss::RfErrorDetection, mip::data_gnss::MIP_GNSS1_DATA_DESC_SET>::initialize()
}},
{MIP_GNSS1_SATELLITE_STATUS_TOPIC, {
FieldWrapperType<mip::data_gnss::SatelliteStatus, mip::data_gnss::MIP_GNSS1_DATA_DESC_SET>::initialize()
}},
{MIP_GNSS1_RAW_TOPIC, {
FieldWrapperType<mip::data_gnss::Raw, mip::data_gnss::MIP_GNSS1_DATA_DESC_SET>::initialize()
}},

// MIP GNSS2 (0x92) topic mappings
{MIP_GNSS2_FIX_INFO_TOPIC, {
Expand All @@ -408,6 +414,12 @@ const std::map<std::string, FieldWrapper::SharedPtrVec> MipPublisherMapping::sta
{MIP_GNSS2_RF_ERROR_DETECTION_TOPIC, {
FieldWrapperType<mip::data_gnss::RfErrorDetection, mip::data_gnss::MIP_GNSS2_DATA_DESC_SET>::initialize()
}},
{MIP_GNSS2_SATELLITE_STATUS_TOPIC, {
FieldWrapperType<mip::data_gnss::SatelliteStatus, mip::data_gnss::MIP_GNSS2_DATA_DESC_SET>::initialize()
}},
{MIP_GNSS2_RAW_TOPIC, {
FieldWrapperType<mip::data_gnss::Raw, mip::data_gnss::MIP_GNSS2_DATA_DESC_SET>::initialize()
}},

// MIP Filter (0x82) topic mappings
{MIP_FILTER_STATUS_TOPIC, {
Expand Down Expand Up @@ -466,11 +478,15 @@ const std::map<std::string, std::string> MipPublisherMapping::static_topic_to_da
{MIP_GNSS1_FIX_INFO_TOPIC, "mip_gnss1_fix_info_data_rate"},
{MIP_GNSS1_SBAS_INFO_TOPIC, "mip_gnss1_sbas_info_data_rate"},
{MIP_GNSS1_RF_ERROR_DETECTION_TOPIC, "mip_gnss1_rf_error_detection_data_rate"},
{MIP_GNSS1_SATELLITE_STATUS_TOPIC, "mip_gnss1_satellite_status_data_rate"},
{MIP_GNSS1_RAW_TOPIC, "mip_gnss1_raw_data_rate"},

// MIP GNSS2 (0x92) data rates
{MIP_GNSS2_FIX_INFO_TOPIC, "mip_gnss2_fix_info_data_rate"},
{MIP_GNSS2_SBAS_INFO_TOPIC, "mip_gnss2_sbas_info_data_rate"},
{MIP_GNSS2_RF_ERROR_DETECTION_TOPIC, "mip_gnss2_rf_error_detection_data_rate"},
{MIP_GNSS2_SATELLITE_STATUS_TOPIC, "mip_gnss2_satellite_status_data_rate"},
{MIP_GNSS2_RAW_TOPIC, "mip_gnss2_raw_data_rate"},

// MIP filter (0x82) data rates
{MIP_FILTER_STATUS_TOPIC, "mip_filter_status_data_rate"},
Expand Down