diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp
index 8d2c0cda37..ed6fead918 100644
--- a/src/lib/l1/ECL_L1_Pos_Controller.cpp
+++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp
@@ -48,12 +48,6 @@
 
 using matrix::Vector2f;
 
-float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
-{
-	/* following [2], switching on L1 distance */
-	return math::min(wp_radius, _L1_distance);
-}
-
 void
 ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
 		const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp
index 0b29d8cb38..c36a2c2731 100644
--- a/src/lib/l1/ECL_L1_Pos_Controller.hpp
+++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp
@@ -97,17 +97,6 @@ public:
 	 */
 	float crosstrack_error() { return _crosstrack_error; }
 
-	/**
-	 * Get the switch distance
-	 *
-	 * This is the distance at which the system will
-	 * switch to the next waypoint. This depends on the
-	 * period and damping
-	 *
-	 * @param waypoint_switch_radius The switching radius the waypoint has set.
-	 */
-	float switch_distance(float waypoint_switch_radius);
-
 	/**
 	 * Navigate between two waypoints
 	 *
diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp
index d7ec54d3e3..861603b6e7 100644
--- a/src/modules/rover_pos_control/RoverPositionControl.cpp
+++ b/src/modules/rover_pos_control/RoverPositionControl.cpp
@@ -45,8 +45,6 @@
 #include "RoverPositionControl.hpp"
 #include <lib/geo/geo.h>
 
-#define ACTUATOR_PUBLISH_PERIOD_MS 4
-
 using namespace matrix;
 
 /**
@@ -266,11 +264,6 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 		float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
 				    (double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
 
-		//PX4_INFO("Setpoint type %d", (int) pos_sp_triplet.current.type );
-		//PX4_INFO(" State machine state %d", (int) _pos_ctrl_state);
-		//PX4_INFO(" Setpoint Lat %f, Lon %f", (double) curr_wp(0), (double)curr_wp(1));
-		//PX4_INFO(" Distance to target %f", (double) dist_target);
-
 		switch (_pos_ctrl_state) {
 		case GOTO_WAYPOINT: {
 				if (dist_target < _param_nav_loiter_rad.get()) {
@@ -305,8 +298,6 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				if (dist_between_waypoints > 0) {
 					_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
 				}
-
-				//PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints);
 			}
 			break;
 
@@ -422,8 +413,6 @@ RoverPositionControl::Run()
 
 				_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
 								     _local_pos.ref_timestamp);
-
-				_global_local_alt0 = _local_pos.ref_alt;
 			}
 
 			// Convert Local setpoints to global setpoints
@@ -435,7 +424,6 @@ RoverPositionControl::Run()
 					_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
 					_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
 
-				_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.position[2];
 				_pos_sp_triplet.current.valid = true;
 			}
 
@@ -451,7 +439,7 @@ RoverPositionControl::Run()
 				if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 
 					//TODO: check if radius makes sense here
-					float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f);
+					float turn_distance = _param_l1_distance.get();
 
 					// publish status
 					position_controller_status_s pos_ctrl_status{};
diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp
index 859cdbd521..31614efe74 100644
--- a/src/modules/rover_pos_control/RoverPositionControl.hpp
+++ b/src/modules/rover_pos_control/RoverPositionControl.hpp
@@ -139,7 +139,6 @@ private:
 	hrt_abstime _manual_setpoint_last_called{0};
 
 	MapProjection _global_local_proj_ref{};
-	float                      _global_local_alt0{NAN};
 
 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp
index 2ea576d0d3..4f03714072 100644
--- a/src/modules/uuv_att_control/uuv_att_control.cpp
+++ b/src/modules/uuv_att_control/uuv_att_control.cpp
@@ -46,9 +46,6 @@
 #include "uuv_att_control.hpp"
 
 
-#define ACTUATOR_PUBLISH_PERIOD_MS 4
-
-
 /**
  * UUV attitude control app start / stop handling function
  *