diff --git a/buoy.orogen b/buoy.orogen
index 4a61318..143e36b 100644
--- a/buoy.orogen
+++ b/buoy.orogen
@@ -394,6 +394,7 @@ task_context "ServoingOnWall" do
 
     property("distance_to_buoy", "double", 1)
     property("angle_to_wall", "double", 0)
+    property("heading_step_size", "double", 0.1) 
 
     property("aligned_distance", "double", 0.5)
 
diff --git a/scripts/debug_gui/run_log_new.rb b/scripts/debug_gui/run_log_new.rb
index d13981d..ebf1a7c 100644
--- a/scripts/debug_gui/run_log_new.rb
+++ b/scripts/debug_gui/run_log_new.rb
@@ -17,7 +17,7 @@
   exit 1
 end
 Orocos.run ("buoy_test") do
-  #Orocos.log_all_ports
+ # Orocos.log_all_ports
   Orocos.conf.load_dir("#{ENV['AUTOPROJ_CURRENT_ROOT']}/bundles/auv/config/orogen/")
   detector = Orocos::TaskContext.get "buoy_detector"
   log = Orocos::Log::Replay.open(ARGV[0], Typelib::Registry.new)
diff --git a/scripts/debug_gui/run_log_new_bottom.rb b/scripts/debug_gui/run_log_new_bottom.rb
index e328050..28a1567 100644
--- a/scripts/debug_gui/run_log_new_bottom.rb
+++ b/scripts/debug_gui/run_log_new_bottom.rb
@@ -24,7 +24,7 @@
 
   log.bottom_camera.frame.connect_to detector.frame
 
-  Orocos.conf.apply(detector, ['bottom_white'])
+  Orocos.conf.apply(detector, ['bottom_orange'])
   detector.configure
   detector.start
 
diff --git a/tasks/ServoingOnWall.cpp b/tasks/ServoingOnWall.cpp
index 040695a..9e09ab6 100644
--- a/tasks/ServoingOnWall.cpp
+++ b/tasks/ServoingOnWall.cpp
@@ -69,11 +69,14 @@ void ServoingOnWall::updateHook()
     if(buoy.validation > 0.8){
         if(last_buoy_validation < 0.8 && state() != BUOY_SERVOING && state() != ALIGNED){
             wall_on_buoy_detection = wall;
+                start_heading = base::getYaw(orientation.orientation);
             state(BUOY_SERVOING);
         }
 
+        double heading_step = base::Angle::normalizeRad(base::getYaw(orientation.orientation) + _heading_step_size.get());
+
         Eigen::Vector3d buoy_rel_pos_in_world = orientation.orientation * buoy.world_coord;
-        Eigen::Vector3d buoy_offset = orientation.orientation.inverse() * (Eigen::AngleAxisd(wall_on_buoy_detection.wall_angle - (M_PI) + _angle_to_wall.get(), Eigen::Vector3d::UnitZ()) * Eigen::Vector3d(_distance_to_buoy.get(),0,0));
+        Eigen::Vector3d buoy_offset = orientation.orientation.inverse() * (Eigen::AngleAxisd(heading_step, Eigen::Vector3d::UnitZ()) * Eigen::Vector3d(_distance_to_buoy.get(),0,0));
         
         buoy.world_coord[0] = buoy.world_coord[0] - 0.8;
 
diff --git a/tasks/ServoingOnWall.hpp b/tasks/ServoingOnWall.hpp
index 2712c45..956ca2b 100644
--- a/tasks/ServoingOnWall.hpp
+++ b/tasks/ServoingOnWall.hpp
@@ -28,6 +28,7 @@ namespace buoy {
         
         sonar_detectors::Wall wall_on_buoy_detection;
         double last_buoy_validation;
+        double start_heading;
 
 
     public: