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

Jump to bottom

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

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

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
088a867
add relposheading in process fix uavcan driver
dirksavage88 Oct 16, 2024
e5e1f05
switch to debug for rtk fix type
dirksavage88 Oct 16, 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
54 changes: 49 additions & 5 deletions src/drivers/uavcan/sensors/gnss.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_sub_auxiliary(node),
_sub_fix(node),
_sub_fix2(node),
_sub_gnss_heading(node),
_pub_moving_baseline_data(node),
_pub_rtcm_stream(node),
_channel_using_fix2(new bool[_max_channels])
Expand Down Expand Up @@ -100,6 +101,15 @@ UavcanGnssBridge::init()
return res;
}

res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb));

if (res < 0) {
PX4_WARN("GNSS relative sub failed %i", res);
return res;
}

// UAVCAN_GPS_YAW_OFFSET
param_get(param_find("GPS_YAW_OFFSET"), &_rel_heading_offset);

// UAVCAN_PUB_RTCM
int32_t uavcan_pub_rtcm = 0;
Expand Down Expand Up @@ -185,12 +195,19 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e

case Fix2::SUB_MODE_RTK_FIXED:
fix_type = 6; // RTK fixed
_rtk_fixed = true;
break;
}

break;
}

// Degraded RTK fix
if (fix_type != 6) {
_rtk_fixed = false;

}

float pos_cov[9] {};
float vel_cov[9] {};
bool valid_covariances = true;
Expand Down Expand Up @@ -295,16 +312,18 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
}
}

// Invalidate the heading fields FIXME:(PX4 CANNODE)
float heading = NAN;
float heading_offset = NAN;
float heading_accuracy = NAN;


int32_t noise_per_ms = -1;
int32_t jamming_indicator = -1;
uint8_t jamming_state = 0;
uint8_t spoofing_state = 0;

// Use ecef_position_velocity for now... There is no heading field
// Use ecef_position_velocity for CANNODE until FIXME above resolved
if (!msg.ecef_position_velocity.empty()) {
if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[0])) {
heading = msg.ecef_position_velocity[0].velocity_xyz[0];
Expand All @@ -328,7 +347,16 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
}
void UavcanGnssBridge::gnss_relative_sub_cb(const
uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg)
{
_last_gnss_relative_timestamp = hrt_absolute_time();

_rel_heading_valid = msg.reported_heading_acc_available;
_rel_heading = math::radians(msg.reported_heading_deg);
_rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg);

}
template <typename FixType>
void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
uint8_t fix_type,
Expand Down Expand Up @@ -436,7 +464,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
}

// If we haven't already done so, set the system clock using GPS data
if ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
if (valid_pos_cov && !_system_clock_set) {
timespec ts{};

// get the whole microseconds
Expand All @@ -463,9 +491,25 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
report.vdop = msg.pdop;
}

report.heading = heading;
report.heading_offset = heading_offset;
report.heading_accuracy = heading_accuracy;
if ((hrt_elapsed_time(&_last_gnss_relative_timestamp) < 2_s) && _rel_heading_valid) {
report.heading = _rel_heading;
report.heading_offset = _rel_heading_offset; // Configured via PX4 uavcan parameter
report.heading_accuracy = _rel_heading_accuracy;

if (!_rtk_fixed) {
PX4_DEBUG("GNSS Fix degraded, RTK heading not stable");

}
}

else {
// Use NAN values if we aren't receiving updated RTK heading

report.heading = heading;
report.heading_offset = heading_offset;
report.heading_accuracy = heading_accuracy;
}


report.noise_per_ms = noise_per_ms;
report.jamming_indicator = jamming_indicator;
Expand Down
18 changes: 17 additions & 1 deletion src/drivers/uavcan/sensors/gnss.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <ardupilot/gnss/MovingBaselineData.hpp>
#include <ardupilot/gnss/RelPosHeading.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp>

#include <lib/perf/perf_counter.h>
Expand Down Expand Up @@ -82,6 +83,8 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
// GNSS Relative Position Heading (AP Periph message)
void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg);

template <typename FixType>
void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
Expand Down Expand Up @@ -113,11 +116,16 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
TimerCbBinder;

typedef uavcan::MethodBinder < UavcanGnssBridge *,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &) >
RelPosHeadingCbBinder;

uavcan::INode &_node;

uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCbBinder> _sub_gnss_heading;

uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _pub_rtcm_stream;
Expand All @@ -137,6 +145,14 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
bool _publish_rtcm_stream{false};
bool _publish_moving_baseline_data{false};

float _rel_heading_accuracy{0.0};
float _rel_heading_offset{0.0};
float _rel_heading{0.0};
bool _rel_heading_valid{false};
uint64_t _last_gnss_relative_timestamp{0};

bool _rtk_fixed{false};

perf_counter_t _rtcm_stream_pub_perf{nullptr};
perf_counter_t _moving_baseline_data_pub_perf{nullptr};
};
};
Loading