fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN Moving Baseline Working

Browse Source 
Set uavcan publisher priorities

Switch to ardupilot rtcm message and add heading accuracy
This commit is contained in:
alexklimaj
2021-11-30 15:59:08 -05:00
committed by Daniel Agar
parent b59db7dac2
commit bfd5a90a5d
23 changed files with 328 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/sensors/gnss.cpp
+29 -9
View File
@@ -98,7 +98,7 @@ UavcanGnssBridge::init()
return res;
}
_pub_rtcm.setPriority(uavcan::TransferPriority::OneHigherThanLowest);
_pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax);
return res;
}
@@ -134,7 +134,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN);
}
void
@@ -274,14 +274,34 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
}
}
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances);
float heading = NAN;
float heading_offset = NAN;
float heading_accuracy = NAN;
// Use ecef_position_velocity for now... There is no heading field
if (!msg.ecef_position_velocity.empty()) {
heading = msg.ecef_position_velocity[0].velocity_xyz[0];
if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
}
if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
}
}
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
heading_accuracy);
}
template <typename FixType>
void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
uint8_t fix_type,
const float (&pos_cov)[9], const float (&vel_cov)[9],
const bool valid_pos_cov, const bool valid_vel_cov)
const bool valid_pos_cov, const bool valid_vel_cov,
const float heading, const float heading_offset,
const float heading_accuracy)
{
sensor_gps_s report{};
report.device_id = get_device_id();
@@ -407,8 +427,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
report.vdop = msg.pdop;
}
report.heading = NAN;
report.heading_offset = NAN;
report.heading = heading;
report.heading_offset = heading_offset;
report.heading_accuracy = heading_accuracy;
publish(msg.getSrcNodeID().get(), &report);
}
@@ -455,12 +476,11 @@ void UavcanGnssBridge::handleInjectDataTopic()
bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len)
{
using uavcan::equipment::gnss::RTCMStream;
using ardupilot::gnss::MovingBaselineData;
perf_count(_rtcm_perf);
RTCMStream msg;
msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;
MovingBaselineData msg;
const size_t capacity = msg.data.capacity();
size_t written = 0;