diff options
Diffstat (limited to 'src/modules/fixedwing_backside')
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing.cpp | 37 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing.hpp | 23 |
2 files changed, 0 insertions, 60 deletions
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 diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index eb5d38381..3876e4630 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -42,10 +42,6 @@ #include <controllib/blocks.hpp> #include <controllib/uorb/blocks.hpp> -extern "C" { -#include <systemlib/geo/geo.h> -} - namespace control { @@ -232,25 +228,6 @@ public: */ /** - * 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; } -}; - -/** * Multi-mode Autopilot */ class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot |