diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
index 2e428f8aa3..cc93613035 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
@@ -32,7 +32,7 @@ protected:
     virtual bool handle_frame(AP_HAL::CANFrame &frame) = 0;
 
     // maximum time between readings before we change state to NoData:
-    virtual uint16_t read_timeout_ms() const { return 200; }
+    virtual uint32_t read_timeout_ms() const { return 200; }
 
     virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
         return MAV_DISTANCE_SENSOR_RADAR;
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
index ba7ce1e8ce..1c922e121b 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
@@ -1,7 +1,8 @@
-#include "AP_RangeFinder_NRA24_CAN.h"
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_NRA24_CAN_ENABLED
 
+#include "AP_RangeFinder_NRA24_CAN.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/AP_HAL.h>
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
index 50eac8c71d..4a114be187 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
@@ -17,7 +17,7 @@ public:
 
 private:
 
-    uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U)/0x10U); }
+    uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); }
 
     static RangeFinder_MultiCAN *multican_NRA24;
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
index bc52cbc7cb..5b9465a1dd 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
@@ -1,8 +1,8 @@
-#include "AP_RangeFinder_TOFSenseP_CAN.h"
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
 
-#include <GCS_MAVLink/GCS.h>
+#include "AP_RangeFinder_TOFSenseP_CAN.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/utility/sparse-endian.h>
 #include <AP_HAL/AP_HAL.h>
@@ -42,7 +42,7 @@ bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame)
         return false;
     }
 
-    const int32_t dist_cm = (int32_t)(frame.data[0] << 8U | frame.data[1] << 16U | frame.data[2] << 24U) / 2560;
+    const int32_t dist_mm = (int32_t)(frame.data[0] << 8U | frame.data[1] << 16U | frame.data[2] << 24U) >> 8;
     const uint8_t status = frame.data[3];
     const uint16_t snr = le16toh_ptr(&frame.data[4]);
 
@@ -51,7 +51,7 @@ bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame)
         return false;
     }
 
-    accumulate_distance_m(dist_cm * 0.01);
+    accumulate_distance_m(dist_mm * 0.001);
     return true;
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h
index 9ca164182d..36c5dcf89c 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h
@@ -121,6 +121,11 @@
 #define AP_RANGEFINDER_NOOPLOOP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
 
+
+#ifndef AP_RANGEFINDER_NRA24_CAN_ENABLED
+#define AP_RANGEFINDER_NRA24_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
+#endif
+
 #ifndef AP_RANGEFINDER_PWM_ENABLED
 #define AP_RANGEFINDER_PWM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif
@@ -137,6 +142,10 @@
 #define AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
+#define AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
+#endif
+
 #ifndef AP_RANGEFINDER_TRI2C_ENABLED
 #define AP_RANGEFINDER_TRI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif
@@ -157,10 +166,3 @@
 #define AP_RANGEFINDER_WASP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif
 
-#ifndef AP_RANGEFINDER_NRA24_CAN_ENABLED
-#define AP_RANGEFINDER_NRA24_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
-#endif
-
-#ifndef AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
-#define AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
-#endif