From a8a8c966101b73465c2e1864b638901cdcf718b8 Mon Sep 17 00:00:00 2001
From: rishabsingh3003 <rishabh.singh100358@gmail.com>
Date: Wed, 19 Jul 2023 13:30:46 +0530
Subject: [PATCH] AP_RangeFinder: small NFC fixes

---
 libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp  | 2 +-
 libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h    | 2 +-
 libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp | 4 ++--
 libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp    | 4 ++--
 libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h      | 8 ++------
 .../AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp      | 8 ++++----
 libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h  | 9 +++------
 libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp     | 2 +-
 libraries/AP_RangeFinder/AP_RangeFinder_config.h         | 8 ++++++++
 9 files changed, 24 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp
index e3abf795b7..96c45d5238 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp
@@ -86,7 +86,7 @@ bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const
 void RangeFinder_MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
 {
     WITH_SEMAPHORE(sem);
-    for (auto *d = drivers; d; d=d->next) {
+    for (auto *d = drivers; d != nullptr; d=d->next) {
         if (d->handle_frame(frame)) {
             break;
         }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
index 6e71afbd4e..2e428f8aa3 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
@@ -42,7 +42,7 @@ protected:
     bool is_correct_id(uint32_t can_id) const;
 
     // set distance and count
-    void set_distance_m(float distance_m) {
+    void accumulate_distance_m(float distance_m) {
         _distance_sum += distance_m;
         _distance_count++;
     };
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp
index 53c2d371fc..180c114f2a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp
@@ -44,7 +44,7 @@ bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame)
     //uint16_t target2 = be16toh_ptr(&frame.data[2]);
     //uint16_t target3 = be16toh_ptr(&frame.data[4]);
 
-    set_distance_m(target1_cm * 0.01);
+    accumulate_distance_m(target1_cm * 0.01);
 
     return true;
 }
@@ -74,7 +74,7 @@ bool AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame)
         return true;
     }
 
-    set_distance_m(dist_cm * 0.01);
+    accumulate_distance_m(dist_cm * 0.01);
     return true;
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
index 61715cc58c..ba7ce1e8ce 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
@@ -1,8 +1,8 @@
 #include "AP_RangeFinder_NRA24_CAN.h"
-#include <AP_BoardConfig/AP_BoardConfig.h>
 
 #if AP_RANGEFINDER_NRA24_CAN_ENABLED
 
+#include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/AP_HAL.h>
 
 RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24;
@@ -73,7 +73,7 @@ bool AP_RangeFinder_NRA24_CAN::handle_frame(AP_HAL::CANFrame &frame)
                 // too low signal strength
                 return false;
             }
-            set_distance_m(dist_m);
+            accumulate_distance_m(dist_m);
         }
             break;
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
index 8ed7d1e6f1..50eac8c71d 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
@@ -1,12 +1,8 @@
 #pragma once
-
-#include "AP_RangeFinder_Backend_CAN.h"
-
-#ifndef AP_RANGEFINDER_NRA24_CAN_ENABLED
-#define AP_RANGEFINDER_NRA24_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
-#endif
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_NRA24_CAN_ENABLED
+#include "AP_RangeFinder_Backend_CAN.h"
 
 class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN {
 public:
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
index 17e43169af..bc52cbc7cb 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
@@ -1,10 +1,10 @@
 #include "AP_RangeFinder_TOFSenseP_CAN.h"
-#include <GCS_MAVLink/GCS.h>
-#include <AP_BoardConfig/AP_BoardConfig.h>
-#include <AP_HAL/utility/sparse-endian.h>
 
 #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
 
+#include <GCS_MAVLink/GCS.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_HAL/utility/sparse-endian.h>
 #include <AP_HAL/AP_HAL.h>
 
 RangeFinder_MultiCAN *AP_RangeFinder_TOFSenseP_CAN::multican_TOFSenseP;
@@ -51,7 +51,7 @@ bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame)
         return false;
     }
 
-    set_distance_m(dist_cm * 0.01);
+    accumulate_distance_m(dist_cm * 0.01);
     return true;
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h
index a08c479fd5..a3100eef09 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h
@@ -1,13 +1,10 @@
 #pragma once
-
-#include "AP_RangeFinder_Backend_CAN.h"
-
-#ifndef AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
-#define AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
-#endif
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
 
+#include "AP_RangeFinder_Backend_CAN.h"
+
 class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN {
 public:
     AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
index 4ad14de2e7..780308aabd 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
@@ -39,7 +39,7 @@ bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
     }
 
     const uint16_t dist_cm = (frame.data[0]<<8) | frame.data[1];
-    set_distance_m(dist_cm * 0.01);
+    accumulate_distance_m(dist_cm * 0.01);
     return true;
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h
index b0aa192eaf..9ca164182d 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h
@@ -156,3 +156,11 @@
 #ifndef AP_RANGEFINDER_WASP_ENABLED
 #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