From dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 22:27:05 -0400 Subject: Segway stabilized. --- src/modules/fixedwing_backside/fixedwing.cpp | 37 ---------------------------- 1 file changed, 37 deletions(-) (limited to 'src/modules/fixedwing_backside/fixedwing.cpp') diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d5dc746e0..f655a13bd 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,43 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _yawDamper.update(rCmd, r, outputScale); } -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))); -} - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique -- cgit v1.2.3