dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num

This commit is contained in:
Andrew Chambers 2014-07-02 19:06:30 -07:00
parent 607b6511a4
commit 6c5e3d5341
2 changed files with 26 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/uavcan/gnss_receiver.cpp
View File

@ -43,7 +43,6 @@
#include <systemlib/err.h>
#define MM_PER_CM 10 // Millimeters per centimeter
#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
_node(node),
@ -79,14 +78,15 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
_report.timestamp_variance = _report.timestamp_position;
// Check that the covariance arrays are valid
bool valid_position_covariance = (msg.position_covariance.size() == EXPECTED_COV_SIZE &&
msg.position_covariance[0] > 0);
bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE &&
msg.velocity_covariance[0] > 0);
// Check if the msg contains valid covariance information
const bool valid_position_covariance = !msg.position_covariance.empty();
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
if (valid_position_covariance) {
_report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4];
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
_report.p_variance_m = std::max(pos_cov[0], pos_cov[4]);
_report.eph_m = sqrtf(_report.p_variance_m);
} else {
_report.p_variance_m = -1.0;
@ -94,7 +94,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
}
if (valid_velocity_covariance) {
_report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8];
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
_report.s_variance_m_s = vel_cov[0] + vel_cov[4] + vel_cov[8];
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
@ -111,9 +113,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
_report.c_variance_rad =
(vel_e_sq * msg.velocity_covariance[0] +
-2 * vel_n * vel_e * msg.velocity_covariance[1] + // Covariance matrix is symmetric
vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
_report.epv_m = sqrtf(msg.position_covariance[8]);

13
src/modules/uavcan/uavcan_main.cpp
View File

@ -243,6 +243,11 @@ int UavcanNode::run()
_node.setStatusOk();
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
@ -253,12 +258,6 @@ int UavcanNode::run()
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@ -272,7 +271,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 1;
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {