From 8f5ba8cd3b98cec6a03d7e062df8cb0fe7c17361 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Tue, 12 Sep 2023 23:32:33 +1000 Subject: [PATCH] wip --- .gitmodules | 3 ++ libraries/AP_RangeFinder/AP_RangeFinder.cpp | 5 ++++ libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder_Broadcom_AFBR_S50.cpp | 21 +++++++++++++ .../AP_RangeFinder_Broadcom_AFBR_S50.h | 30 +++++++++++++++++++ libraries/AP_RangeFinder/wscript | 27 +++++++++++++++++ modules/drivers/broadcom/AFBR-S50 | 1 + wscript | 2 ++ 8 files changed, 90 insertions(+) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.h create mode 100644 libraries/AP_RangeFinder/wscript create mode 160000 modules/drivers/broadcom/AFBR-S50 diff --git a/.gitmodules b/.gitmodules index fbfe76753a51d..b58d396eab287 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,3 +39,6 @@ path = modules/Micro-CDR url = https://github.com/ardupilot/Micro-CDR.git branch = master +[submodule "modules/drivers/broadcom/AFBR-S50"] + path = modules/drivers/broadcom/AFBR-S50 + url = git@github.com:Broadcom/AFBR-S50-API.git diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 7376069717e8e..e575109a7800a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -55,6 +55,7 @@ #include "AP_RangeFinder_NoopLoop.h" #include "AP_RangeFinder_TOFSenseP_CAN.h" #include "AP_RangeFinder_NRA24_CAN.h" +#include "AP_RangeFinder_Broadcom_AFBR_S50.h" #include #include @@ -556,6 +557,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance); #endif break; + case Type::Broadcom_AFBRS50: + if (AP_RangeFinder_Broadcom_AFBRS50::detect()) { + _add_backend(new AP_RangeFinder_Broadcom_AFBRS50(state[instance], params[instance]), instance); + } case Type::NONE: break; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7365ed3912cdf..56053b01ab04a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -96,6 +96,7 @@ class RangeFinder NoopLoop_P = 37, TOFSenseP_CAN = 38, NRA24_CAN = 39, + Broadcom_AFBRS50 = 40, SIM = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.cpp new file mode 100644 index 0000000000000..15e724a8719f8 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.cpp @@ -0,0 +1,21 @@ +#include "AP_RangeFinder_config.h" + +#include "AP_RangeFinder_Broadcom_AFBR_S50.h" +#include + +// constructor +AP_RangeFinder_Broadcom_AFBRS50::AP_RangeFinder_Broadcom_AFBRS50(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) +{ + _hnd = Argus_CreateHandle(); +} + +bool AP_RangeFinder_Broadcom_AFBRS50::detect() +{ + return true; +} + +void AP_RangeFinder_Broadcom_AFBRS50::update(void) +{ + +} \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.h b/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.h new file mode 100644 index 0000000000000..46e92c029ae39 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Broadcom_AFBR_S50.h @@ -0,0 +1,30 @@ +#pragma once + +#include "argus.h" + +#include "AP_RangeFinder_Backend.h" + +class AP_RangeFinder_Broadcom_AFBRS50 : public AP_RangeFinder_Backend +{ +public: + + // constructor + AP_RangeFinder_Broadcom_AFBRS50(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + // detect + static bool detect(); + + // update state + void update(void) override; + +private: + float _distance_m; // stored data + + argus_hnd_t *_hnd; + argus_mode_t _mode{ARGUS_MODE_SHORT_RANGE}; // Short-Range + +protected: + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } +}; diff --git a/libraries/AP_RangeFinder/wscript b/libraries/AP_RangeFinder/wscript new file mode 100644 index 0000000000000..9eb36ffd71d2d --- /dev/null +++ b/libraries/AP_RangeFinder/wscript @@ -0,0 +1,27 @@ + + +from waflib.TaskGen import after_method, before_method, feature +import os +import shutil + + +def configure(cfg): + """test""" + + +def build(bld): + RANGEFINDER_LIBS = { + 'cortex-m4' : 'libafbrs50_m4_fpu.a' + } + if bld.env.CORTEX in RANGEFINDER_LIBS: + path_AFBR_S50 = 'modules/drivers/broadcom/AFBR-S50/AFBR-S50' + libname = RANGEFINDER_LIBS[bld.env.CORTEX] + # we need to copy the library on cygwin as it doesn't handle linking outside build tree + shutil.copyfile(os.path.join(bld.env.SRCROOT, path_AFBR_S50, "Lib", libname), + os.path.join(bld.env.BUILDROOT,'libraries/AP_RangeFinder/libAFBR-S50-API.a')) + bld.env.LIB += ['AFBR-S50-API'] + bld.env.LIBPATH += ['libraries/AP_RangeFinder'] + bld.env.INCLUDES += [os.path.join(bld.env.SRCROOT, path_AFBR_S50, "Include")] + + + diff --git a/modules/drivers/broadcom/AFBR-S50 b/modules/drivers/broadcom/AFBR-S50 new file mode 160000 index 0000000000000..945e3abc66af2 --- /dev/null +++ b/modules/drivers/broadcom/AFBR-S50 @@ -0,0 +1 @@ +Subproject commit 945e3abc66af2d0dc5b3229b6ef7c0d24ee919cd diff --git a/wscript b/wscript index db52f181a7ddd..4e8125748c5fe 100644 --- a/wscript +++ b/wscript @@ -538,6 +538,7 @@ def configure(cfg): else: cfg.end_msg('maybe') cfg.recurse('libraries/AP_Scripting') + cfg.recurse('libraries/AP_RangeFinder') cfg.recurse('libraries/AP_GPS') cfg.recurse('libraries/AP_HAL_SITL') @@ -791,6 +792,7 @@ def _build_recursion(bld): dirs_to_recurse.append('Tools/AP_Periph') dirs_to_recurse.append('libraries/AP_Scripting') + dirs_to_recurse.append('libraries/AP_RangeFinder') if bld.env.ENABLE_ONVIF: dirs_to_recurse.append('libraries/AP_ONVIF')