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