Segway stabilized.

2 files changed, 60 insertions, 0 deletions

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"
@@ -65,6 +69,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
  */
 class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock