diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 25e9112fa8..69b67be377 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1148,7 +1148,7 @@ static void loadMainState(timeUs_t currentTimeUs)
 
 #ifdef USE_RANGEFINDER
     // Store the raw rangefinder surface readout without applying tilt correction
-    blackboxCurrent->surfaceRaw = rangefinderRead();
+    blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
 #endif
 
     blackboxCurrent->rssi = rssi;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 0f0e2b1070..c4231de403 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -184,7 +184,12 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
         rescheduleTask(TASK_SELF, newDeadline);
     }
 
-    updatePositionEstimator_SurfaceTopic(currentTimeUs);
+    /*
+     * Process raw rangefinder readout
+     */
+    rangefinderProcess(calculateCosTiltAngle());
+    
+    updatePositionEstimator_SurfaceTopic(currentTimeUs, rangefinderGetLatestAltitude());
 }
 #endif
 
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index af74308e03..01de57cbe5 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -248,7 +248,7 @@ void navigationInit(void);
 
 /* Position estimator update functions */
 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
-void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs);
+void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt);
 
 /* Navigation system updates */
 void updateWaypointsAndNavigationMode(void);
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 0f4b5803c2..dc86853b64 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -474,11 +474,8 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
  * Read surface and update alt/vel topic
  *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
  */
-void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs)
+void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
 {
-    float newSurfaceAlt = rangefinderRead();
-    newSurfaceAlt = rangefinderCalculateAltitude(newSurfaceAlt, calculateCosTiltAngle());
-
     if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
         posEstimator.surface.alt = newSurfaceAlt;
         posEstimator.surface.lastUpdateTime = currentTimeUs;
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index 3a5ccbd68e..3f57422e1e 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -188,7 +188,7 @@ timeDelta_t rangefinderUpdate(void)
 /**
  * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
  */
-int32_t rangefinderRead(void)
+void rangefinderProcess(float cosTiltAngle)
 {
     if (rangefinder.dev.read) {
         const int32_t distance = rangefinder.dev.read();
@@ -213,31 +213,24 @@ int32_t rangefinderRead(void)
         rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
     }
 
-    DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
-
-    return rangefinder.rawAltitude;
-}
-
-/**
- * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
- * the altitude. Returns the computed altitude in centimeters.
- *
- * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
- */
-int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle)
-{
-    // calculate sonar altitude only if the ground is in the sonar cone
-    if (cosTiltAngle < rangefinder.maxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
+    /**
+    * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
+    * the altitude. Returns the computed altitude in centimeters.
+    *
+    * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
+    */
+    if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
         rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
     } else {
-        rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle;
+        rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
     }
+
+    DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
     DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
-    return rangefinder.calculatedAltitude;
 }
 
 /**
- * Get the latest altitude that was computed by a call to rangefinderCalculateAltitude(), or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
+ * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
  * has never been called.
  */
 int32_t rangefinderGetLatestAltitude(void)
@@ -245,6 +238,10 @@ int32_t rangefinderGetLatestAltitude(void)
     return rangefinder.calculatedAltitude;
 }
 
+int32_t rangefinderGetLatestRawAltitude(void) {
+    return rangefinder.rawAltitude;
+}
+
 bool rangefinderIsHealthy(void)
 {
     return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index 578d50642f..3d931fd2ec 100644
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@@ -48,11 +48,9 @@ const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void);
 
 bool rangefinderInit(void);
 
-
-
-int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle);
 int32_t rangefinderGetLatestAltitude(void);
+int32_t rangefinderGetLatestRawAltitude(void);
 
 timeDelta_t rangefinderUpdate(void);
-int32_t rangefinderRead(void);
+void rangefinderProcess(float cosTiltAngle);
 bool rangefinderIsHealthy(void);
diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c
index 94b3766054..4a50dc6388 100755
--- a/src/main/target/MATEKF405/target.c
+++ b/src/main/target/MATEKF405/target.c
@@ -24,14 +24,13 @@
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM5, IO_TAG(PA3),    TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PPM },
 
-    { TIM3, IO_TAG(PC6),    TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR },
-    { TIM8, IO_TAG(PC7),    TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
-    { TIM8, IO_TAG(PC8),    TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
-    { TIM8, IO_TAG(PC9),    TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
-    
-    { TIM2, IO_TAG(PA15),   TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
-    { TIM1, IO_TAG(PA8),    TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
+    { TIM3, IO_TAG(PC6),    TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR },
+    { TIM8, IO_TAG(PC7),    TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
+    { TIM8, IO_TAG(PC8),    TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
+    { TIM8, IO_TAG(PC9),    TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
+
+    { TIM2, IO_TAG(PA15),   TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
+    { TIM1, IO_TAG(PA8),    TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO },
 
     { TIM4, IO_TAG(PB6),    TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_LED }
 };
-