diff options
author | Andrew Chambers <achamber@gmail.com> | 2014-07-02 19:06:30 -0700 |
---|---|---|
committer | Andrew Chambers <achamber@gmail.com> | 2014-07-02 19:06:30 -0700 |
commit | 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca (patch) | |
tree | 09d84c97c5da08ea1edee8d4c5528e56ec0bf120 | |
parent | 607b6511a413b5bc2b2b0ae350a9451e83da9803 (diff) | |
download | px4-firmware-6c5e3d53412fa1cdad687818328b3bfc1a83e9ca.tar.gz px4-firmware-6c5e3d53412fa1cdad687818328b3bfc1a83e9ca.tar.bz2 px4-firmware-6c5e3d53412fa1cdad687818328b3bfc1a83e9ca.zip |
Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num
-rw-r--r-- | src/modules/uavcan/gnss_receiver.cpp | 28 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 23 |
2 files changed, 26 insertions, 25 deletions
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 23c221565..65a7b4a2a 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -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), @@ -75,18 +74,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; _report.lon = msg.lon_1e7; - _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) _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]); @@ -138,7 +140,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav _report.timestamp_satellites = _report.timestamp_position; _report.satellites_visible = msg.sats_used; - _report.satellite_info_available = 0; // Set to 0 for no info available + _report.satellite_info_available = 0; // Set to 0 for no info available if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c0c07be53..5bc437670 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,22 +243,21 @@ int UavcanNode::run() _node.setStatusOk(); - /* - * 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 - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _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; + /* + * 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 + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; } 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) { |