From 8acbe6d5b6770e92fdcb86ba268492217d3e26bd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Tue, 1 Jul 2014 14:08:59 -0700 Subject: [PATCH 1/6] Added class to convert gnss message from uavcan to uorb --- src/modules/uavcan/gnss_receiver.cpp | 122 +++++++++++++++++++++++++++ src/modules/uavcan/gnss_receiver.hpp | 86 +++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 7 +- src/modules/uavcan/uavcan_main.hpp | 2 + 4 files changed, 216 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/gnss_receiver.cpp create mode 100644 src/modules/uavcan/gnss_receiver.hpp diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp new file mode 100644 index 0000000000..e924683330 --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gnss_receiver.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss_receiver.hpp" +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : + _node(node), + _uavcan_sub_status(node), + _report_pub(-1) +{ +} + +int UavcanGnssReceiver::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + return res; +} + +void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _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.timestamp_variance = _report.timestamp_position; + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + + /* Use Jacobian to transform velocity covariance to heading covariance + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * var(heading) = J(velocity)*var(velocity)*J(velocity)^T + */ + _report.c_variance_rad = + msg.ned_velocity[1] * msg.ned_velocity[1] * msg.velocity_covariance[0] + + -2*msg.ned_velocity[1] * msg.ned_velocity[0] * msg.velocity_covariance[1] + + msg.ned_velocity[0] * msg.ned_velocity[0] * msg.velocity_covariance[4]; + + _report.fix_type = msg.status; + + _report.eph_m = sqrtf(_report.p_variance_m); + _report.epv_m = sqrtf(msg.position_covariance[8]); + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = msg.timestamp.husec * msg.timestamp.USEC_PER_LSB; // Convert to microseconds + + _report.timestamp_satellites = _report.timestamp_position; + _report.satellites_visible = msg.sats_used; + _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); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp new file mode 100644 index 0000000000..abb8a821af --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gnss_receiver.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +class UavcanGnssReceiver +{ +public: + UavcanGnssReceiver(uavcan::INode& node); + + int init(); + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + typedef uavcan::MethodBinder&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ab687a6b9d..5bc4376709 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -63,7 +63,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node) + _esc_controller(_node), + _gnss_receiver(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -186,6 +187,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; + ret = _gnss_receiver.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 751a94a8a7..126d441377 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -43,6 +43,7 @@ #include #include "esc_controller.hpp" +#include "gnss_receiver.hpp" /** * @file uavcan_main.hpp @@ -108,6 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c6de9395818717916bbc1077fecd51c4db87936 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 10:04:07 -0700 Subject: [PATCH 2/6] Fixed heading covariance calculation and build errors. --- src/modules/uavcan/gnss_receiver.cpp | 17 ++++++++++++----- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index e924683330..490e35bd17 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -80,17 +80,24 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 2 Jul 2014 11:18:30 -0700 Subject: [PATCH 3/6] Fixed bug with zero-sized covariance arrays --- src/modules/uavcan/gnss_receiver.cpp | 75 ++++++++++++++++++---------- src/modules/uavcan/uavcan_main.cpp | 23 +++++---- 2 files changed, 60 insertions(+), 38 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 490e35bd17..23c2215652 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -42,12 +42,13 @@ #include "gnss_receiver.hpp" #include -#define MM_PER_CM 10 // Millimeters per centimeter +#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), - _uavcan_sub_status(node), - _report_pub(-1) +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) { } @@ -77,33 +78,53 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); + bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && + msg.velocity_covariance[0] > 0); + + if (valid_position_covariance) { + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + _report.eph_m = sqrtf(_report.p_variance_m); + } else { + _report.p_variance_m = -1.0; + _report.eph_m = -1.0; + } + + if (valid_velocity_covariance) { + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + 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)); + + _report.epv_m = sqrtf(msg.position_covariance[8]); + + } else { + _report.s_variance_m_s = -1.0; + _report.c_variance_rad = -1.0; + _report.epv_m = -1.0; + } _report.fix_type = msg.status; - _report.eph_m = sqrtf(_report.p_variance_m); - _report.epv_m = sqrtf(msg.position_covariance[8]); - _report.timestamp_velocity = _report.timestamp_position; _report.vel_n_m_s = msg.ned_velocity[0]; _report.vel_e_m_s = msg.ned_velocity[1]; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc4376709..c0c07be536 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,21 +243,22 @@ 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); @@ -271,7 +272,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { From 607b6511a413b5bc2b2b0ae350a9451e83da9803 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 11:27:49 -0700 Subject: [PATCH 4/6] Fixed comments --- src/modules/uavcan/gnss_receiver.hpp | 6 ++---- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp index abb8a821af..18df8da2f5 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -34,10 +34,8 @@ /** * @file gnss_receiver.hpp * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix * * @author Pavel Kirienko * @author Andrew Chambers diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 126d441377..443525379e 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -109,7 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 19:06:30 -0700 Subject: [PATCH 5/6] Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num --- src/modules/uavcan/gnss_receiver.cpp | 28 +++++++++++++++------------- 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 23c2215652..65a7b4a2a3 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -43,7 +43,6 @@ #include #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 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 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 c0c07be536..5bc4376709 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) { From c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 3 Jul 2014 11:32:27 -0700 Subject: [PATCH 6/6] Using proper math library. Corrected speed variance calculation --- src/modules/uavcan/gnss_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 65a7b4a2a3..3e98bdf144 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -41,6 +41,7 @@ #include "gnss_receiver.hpp" #include +#include #define MM_PER_CM 10 // Millimeters per centimeter @@ -86,7 +87,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure