diff options
author | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
commit | dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 (patch) | |
tree | a3a3715da3a9bac871241df71a79d7687b862e3f /src/modules/controllib/uorb | |
parent | 1980d9dd63e29390f7c3ba9b31be576c07706f73 (diff) | |
download | px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.gz px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.bz2 px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.zip |
Segway stabilized.
Diffstat (limited to 'src/modules/controllib/uorb')
-rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 37 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.hpp | 23 |
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 |