Added class to convert gnss message from uavcan to uorb

4 files changed, 216 insertions, 1 deletions

diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp
new file mode 100644
index 000000000..e92468333
--- /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 <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ *
+ */
+
+#include "gnss_receiver.hpp"
+#include <systemlib/err.h>
+
+#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<uavcan::equipment::gnss::Fix> &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 000000000..abb8a821a
--- /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 <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Fix.hpp>
+
+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<uavcan::equipment::gnss::Fix> &msg);
+
+
+	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
+		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+		FixCbBinder;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::INode													&_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_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 ab687a6b9..5bc437670 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 751a94a8a..126d44137 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -43,6 +43,7 @@
 #include <uORB/topics/actuator_armed.h>
 
 #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;