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/blocks.cpp | |
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/blocks.cpp')
-rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 37 |
1 files changed, 37 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 |