From dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sun, 28 Jul 2013 22:27:05 -0400
Subject: Segway stabilized.

---
 src/modules/controllib/uorb/blocks.cpp | 37 ++++++++++++++++++++++++++++++++++
 src/modules/controllib/uorb/blocks.hpp | 23 +++++++++++++++++++++
 2 files changed, 60 insertions(+)

(limited to 'src/modules/controllib')

diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index 6e5ade519..448a42a99 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -42,6 +42,43 @@
 namespace control
 {
 
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
 BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// subscriptions
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 54f31735c..9c0720aa5 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -57,6 +57,10 @@
 #include <drivers/drv_hrt.h>
 #include <poll.h>
 
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
 #include "../blocks.hpp"
 #include "UOrbSubscription.hpp"
 #include "UOrbPublication.hpp"
@@ -64,6 +68,25 @@
 namespace control
 {
 
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
 /**
  * UorbEnabledAutopilot
  */
-- 
cgit v1.2.3