From 6025b1dcaa591c583d4c34887c371449b6e5934d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 5 Jul 2022 16:28:24 +0900 Subject: [PATCH] AP_Proximity: move params to separate class simplies increasing the maximum number of backends --- libraries/AP_Proximity/AP_Proximity.cpp | 210 +++++------------- libraries/AP_Proximity/AP_Proximity.h | 29 +-- .../AP_Proximity/AP_Proximity_Backend.cpp | 17 +- libraries/AP_Proximity/AP_Proximity_Backend.h | 3 +- .../AP_Proximity_Backend_Serial.cpp | 9 +- .../AP_Proximity_Backend_Serial.h | 6 +- .../AP_Proximity/AP_Proximity_Boundary_3D.h | 2 +- .../AP_Proximity_LightWareSF45B.h | 5 +- .../AP_Proximity/AP_Proximity_Params.cpp | 148 ++++++++++++ libraries/AP_Proximity/AP_Proximity_Params.h | 27 +++ libraries/AP_Proximity/AP_Proximity_SITL.cpp | 5 +- libraries/AP_Proximity/AP_Proximity_SITL.h | 2 +- 12 files changed, 273 insertions(+), 190 deletions(-) create mode 100644 libraries/AP_Proximity/AP_Proximity_Params.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_Params.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 174c386eb0..a90780efb1 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -35,124 +35,10 @@ extern const AP_HAL::HAL &hal; const AP_Param::GroupInfo AP_Proximity::var_info[] = { // 0 is reserved for possible addition of an ENABLED parameter - // @Param: _TYPE - // @DisplayName: Proximity type - // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1 - // @RebootRequired: True - // @User: Standard - AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity, _type[0], 0, AP_PARAM_FLAG_ENABLE), - - // @Param: _ORIENT - // @DisplayName: Proximity sensor orientation - // @Description: Proximity sensor orientation - // @Values: 0:Default,1:Upside Down - // @User: Standard - AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0), - - // @Param: _YAW_CORR - // @DisplayName: Proximity sensor yaw correction - // @Description: Proximity sensor yaw correction - // @Units: deg - // @Range: -180 180 - // @User: Standard - AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], 0), - - // @Param: _IGN_ANG1 - // @DisplayName: Proximity sensor ignore angle 1 - // @Description: Proximity sensor ignore angle 1 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0), - - // @Param: _IGN_WID1 - // @DisplayName: Proximity sensor ignore width 1 - // @Description: Proximity sensor ignore width 1 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0), - - // @Param: _IGN_ANG2 - // @DisplayName: Proximity sensor ignore angle 2 - // @Description: Proximity sensor ignore angle 2 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0), - - // @Param: _IGN_WID2 - // @DisplayName: Proximity sensor ignore width 2 - // @Description: Proximity sensor ignore width 2 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0), - - // @Param: _IGN_ANG3 - // @DisplayName: Proximity sensor ignore angle 3 - // @Description: Proximity sensor ignore angle 3 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0), - - // @Param: _IGN_WID3 - // @DisplayName: Proximity sensor ignore width 3 - // @Description: Proximity sensor ignore width 3 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0), - - // @Param: _IGN_ANG4 - // @DisplayName: Proximity sensor ignore angle 4 - // @Description: Proximity sensor ignore angle 4 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0), - - // @Param: _IGN_WID4 - // @DisplayName: Proximity sensor ignore width 4 - // @Description: Proximity sensor ignore width 4 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0), - - // @Param: _IGN_ANG5 - // @DisplayName: Proximity sensor ignore angle 5 - // @Description: Proximity sensor ignore angle 5 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0), - - // @Param: _IGN_WID5 - // @DisplayName: Proximity sensor ignore width 5 - // @Description: Proximity sensor ignore width 5 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0), - - // @Param: _IGN_ANG6 - // @DisplayName: Proximity sensor ignore angle 6 - // @Description: Proximity sensor ignore angle 6 - // @Units: deg - // @Range: 0 360 - // @User: Standard - AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0), - - // @Param: _IGN_WID6 - // @DisplayName: Proximity sensor ignore width 6 - // @Description: Proximity sensor ignore width 6 - // @Units: deg - // @Range: 0 127 - // @User: Standard - AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0), + // 1 was _TYPE + // 2 was _ORIENT + // 3 was _YAW_CORR + // 4 to 15 was _IGN_ANG1 to _IGN_WID6 // @Param{Copter}: _IGN_GND // @DisplayName: Proximity sensor land detection @@ -176,21 +62,30 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @User: Advanced AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f), - // @Param: _MIN - // @DisplayName: Proximity minimum range - // @Description: Minimum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range. - // @Units: m - // @Range: 0 500 - // @User: Advanced - AP_GROUPINFO("_MIN", 19, AP_Proximity, _min_m, 0.0f), + // 19 was _MIN + // 20 was _MAX - // @Param: _MAX - // @DisplayName: Proximity maximum range - // @Description: Maximum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range. - // @Units: m - // @Range: 0 500 - // @User: Advanced - AP_GROUPINFO("_MAX", 20, AP_Proximity, _max_m, 0.0f), + // @Group: 1 + // @Path: AP_Proximity_Params.cpp + AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params), + +#if PROXIMITY_MAX_INSTANCES > 1 + // @Group: 2 + // @Path: AP_Proximity_Params.cpp + AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params), +#endif + +#if PROXIMITY_MAX_INSTANCES > 2 + // @Group: 3 + // @Path: AP_Proximity_Params.cpp + AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params), +#endif + +#if PROXIMITY_MAX_INSTANCES > 3 + // @Group: 4 + // @Path: AP_Proximity_Params.cpp + AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params), +#endif AP_GROUPEND }; @@ -253,7 +148,7 @@ uint8_t AP_Proximity::get_orientation(uint8_t instance) const return 0; } - return _orientation[instance].get(); + return params[instance].orientation.get(); } // return sensor yaw correction @@ -263,7 +158,7 @@ int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const return 0; } - return _yaw_correction[instance].get(); + return params[instance].yaw_correction.get(); } // return sensor health @@ -292,6 +187,19 @@ void AP_Proximity::handle_msg(const mavlink_message_t &msg) } } +// return true if the given instance exists +bool AP_Proximity::valid_instance(uint8_t i) const +{ + if (i >= PROXIMITY_MAX_INSTANCES) { + return false; + } + + if (drivers[i] == nullptr) { + return false; + } + return (Type)params[i].type.get() != Type::None; +} + // detect if an instance of a proximity sensor is connected. void AP_Proximity::detect_instance(uint8_t instance) { @@ -299,58 +207,58 @@ void AP_Proximity::detect_instance(uint8_t instance) case Type::None: return; case Type::RPLidarA2: - if (AP_Proximity_RPLidarA2::detect()) { + if (AP_Proximity_RPLidarA2::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]); + drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance]); return; } break; case Type::MAV: state[instance].instance = instance; - drivers[instance] = new AP_Proximity_MAV(*this, state[instance]); + drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]); return; case Type::TRTOWER: - if (AP_Proximity_TeraRangerTower::detect()) { + if (AP_Proximity_TeraRangerTower::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance]); + drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]); return; } break; case Type::TRTOWEREVO: - if (AP_Proximity_TeraRangerTowerEvo::detect()) { + if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance]); + drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance]); return; } break; case Type::RangeFinder: state[instance].instance = instance; - drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]); + drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]); return; case Type::SF40C: - if (AP_Proximity_LightWareSF40C::detect()) { + if (AP_Proximity_LightWareSF40C::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance]); + drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]); return; } break; case Type::SF45B: - if (AP_Proximity_LightWareSF45B::detect()) { + if (AP_Proximity_LightWareSF45B::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance]); + drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance]); return; } break; case Type::CYGBOT_D1: #if AP_PROXIMITY_CYGBOT_ENABLED - if (AP_Proximity_Cygbot_D1::detect()) { + if (AP_Proximity_Cygbot_D1::detect(instance)) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance]); + drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]); return; } # endif @@ -359,12 +267,12 @@ void AP_Proximity::detect_instance(uint8_t instance) #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case Type::SITL: state[instance].instance = instance; - drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); + drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]); return; case Type::AirSimSITL: state[instance].instance = instance; - drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]); + drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]); return; #endif @@ -496,7 +404,7 @@ bool AP_Proximity::get_upward_distance(float &distance) const AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const { if (instance < PROXIMITY_MAX_INSTANCES) { - return (Type)((uint8_t)_type[instance]); + return (Type)((uint8_t)params[instance].type); } return Type::None; } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 786d0c3fb8..4e8301e697 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -22,9 +22,9 @@ #include #include #include +#include "AP_Proximity_Params.h" -#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform -#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored +#define PROXIMITY_MAX_INSTANCES 2 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_DIRECTION 8 #define PROXIMITY_SENSOR_ID_START 10 @@ -164,6 +164,11 @@ public: // messages: void log(); +protected: + + // parameters for backends + AP_Proximity_Params params[PROXIMITY_MAX_INSTANCES]; + private: static AP_Proximity *_singleton; Proximity_State state[PROXIMITY_MAX_INSTANCES]; @@ -171,27 +176,15 @@ private: uint8_t primary_instance; uint8_t num_instances; - bool valid_instance(uint8_t i) const { - if (drivers[i] == nullptr) { - return false; - } - return (Type)_type[i].get() != Type::None; - } + // return true if the given instance exists + bool valid_instance(uint8_t i) const; + + void detect_instance(uint8_t instance); // parameters for all instances - AP_Int8 _type[PROXIMITY_MAX_INSTANCES]; - AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES]; - AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES]; - AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up) - AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored AP_Int8 _raw_log_enable; // enable logging raw distances AP_Int8 _ign_gnd_enable; // true if land detection should be enabled AP_Float _filt_freq; // cutoff frequency for low pass filter - AP_Float _max_m; // Proximity maximum range - AP_Float _min_m; // Proximity minimum range - - void detect_instance(uint8_t instance); - }; namespace AP { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 55478aef1c..386f55f351 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -28,9 +28,10 @@ extern const AP_HAL::HAL& hal; base class constructor. This incorporates initialisation as well. */ -AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : +AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity::Proximity_State& _state, AP_Proximity_Params& _params) : frontend(_frontend), - state(_state) + state(_state), + params(_params) { } @@ -85,15 +86,15 @@ float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) c bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area) const { // check if distances are supposed to be in a particular range - if (!is_zero(frontend._max_m)) { - if (distance_m > frontend._max_m) { + if (!is_zero(params.max_m)) { + if (distance_m > params.max_m) { // too far away return true; } } - if (!is_zero(frontend._min_m)) { - if (distance_m < frontend._min_m) { + if (!is_zero(params.min_m)) { + if (distance_m < params.min_m) { // too close return true; } @@ -102,8 +103,8 @@ bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance if (check_for_ign_area) { // check angle vs each ignore area for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) { - if (frontend._ignore_width_deg[i] != 0) { - if (abs(yaw - frontend._ignore_angle_deg[i]) <= (frontend._ignore_width_deg[i]/2)) { + if (params.ignore_width_deg[i] != 0) { + if (abs(yaw - params.ignore_angle_deg[i]) <= (params.ignore_width_deg[i]/2)) { return true; } } diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 1938976662..d7d1b741af 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -28,7 +28,7 @@ class AP_Proximity_Backend { public: // constructor. This incorporates initialisation as well. - AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params &_params); // we declare a virtual destructor so that Proximity drivers can // override with a custom destructor if need be @@ -121,6 +121,7 @@ protected: AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state + AP_Proximity_Params ¶ms; // parameters for this backend // Methods to manipulate 3D boundary in this class AP_Proximity_Boundary_3D boundary; diff --git a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp index 4d77598cce..7bb9bb2462 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp @@ -24,8 +24,9 @@ already know that we should setup the proximity sensor */ AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state) : - AP_Proximity_Backend(_frontend, _state) + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params &_params) : + AP_Proximity_Backend(_frontend, _state, _params) { const AP_SerialManager &serial_manager = AP::serialmanager(); _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); @@ -37,9 +38,9 @@ AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend // detect if a proximity sensor is connected by looking for a // configured serial port -bool AP_Proximity_Backend_Serial::detect() +bool AP_Proximity_Backend_Serial::detect(uint8_t instance) { - return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); + return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, instance); } #endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h index af05334509..e0164d6239 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h @@ -8,9 +8,11 @@ class AP_Proximity_Backend_Serial : public AP_Proximity_Backend { public: AP_Proximity_Backend_Serial(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state); + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params); + // static detection function - static bool detect(); + static bool detect(uint8_t instance); protected: virtual uint16_t rxspace() const { return 0; }; diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index ae46e7b0a0..ab6e3039f1 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -164,7 +164,7 @@ private: }; // This class gives an easy way of making a temporary boundary, used for "sorting" distances. -// When unkown number of distances at various orientations are sent we store the least distance in the temporary boundary. +// When unknown number of distances at various orientations are sent we store the least distance in the temporary boundary. // After all the messages are received, we copy the contents of the temporary boundary and put it in the main 3-D boundary. class AP_Proximity_Temp_Boundary { diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h index 8438126d24..95dc0eb65b 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h @@ -11,8 +11,9 @@ class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial public: // constructor AP_Proximity_LightWareSF45B(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state) : - AP_Proximity_LightWareSerial(_frontend, _state) {} + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params) : + AP_Proximity_LightWareSerial(_frontend, _state, _params) {} uint16_t rxspace() const override { return 1280; diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp new file mode 100644 index 0000000000..1cde62b3d2 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -0,0 +1,148 @@ +#include "AP_Proximity_Params.h" + +// table of user settable parameters +const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { + + // 0 should not be used + + // @Param: _TYPE + // @DisplayName: Proximity type + // @Description: What type of proximity sensor is connected + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1 + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: _ORIENT + // @DisplayName: Proximity sensor orientation + // @Description: Proximity sensor orientation + // @Values: 0:Default,1:Upside Down + // @User: Standard + AP_GROUPINFO("_ORIENT", 2, AP_Proximity_Params, orientation, 0), + + // @Param: _YAW_CORR + // @DisplayName: Proximity sensor yaw correction + // @Description: Proximity sensor yaw correction + // @Units: deg + // @Range: -180 180 + // @User: Standard + AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity_Params, yaw_correction, 0), + + // @Param: _IGN_ANG1 + // @DisplayName: Proximity sensor ignore angle 1 + // @Description: Proximity sensor ignore angle 1 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity_Params, ignore_angle_deg[0], 0), + + // @Param: _IGN_WID1 + // @DisplayName: Proximity sensor ignore width 1 + // @Description: Proximity sensor ignore width 1 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity_Params, ignore_width_deg[0], 0), + + // @Param: _IGN_ANG2 + // @DisplayName: Proximity sensor ignore angle 2 + // @Description: Proximity sensor ignore angle 2 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity_Params, ignore_angle_deg[1], 0), + + // @Param: _IGN_WID2 + // @DisplayName: Proximity sensor ignore width 2 + // @Description: Proximity sensor ignore width 2 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity_Params, ignore_width_deg[1], 0), + + // @Param: _IGN_ANG3 + // @DisplayName: Proximity sensor ignore angle 3 + // @Description: Proximity sensor ignore angle 3 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity_Params, ignore_angle_deg[2], 0), + + // @Param: _IGN_WID3 + // @DisplayName: Proximity sensor ignore width 3 + // @Description: Proximity sensor ignore width 3 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity_Params, ignore_width_deg[2], 0), + + // @Param: _IGN_ANG4 + // @DisplayName: Proximity sensor ignore angle 4 + // @Description: Proximity sensor ignore angle 4 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity_Params, ignore_angle_deg[3], 0), + + // @Param: _IGN_WID4 + // @DisplayName: Proximity sensor ignore width 4 + // @Description: Proximity sensor ignore width 4 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity_Params, ignore_width_deg[3], 0), + + // @Param: _IGN_ANG5 + // @DisplayName: Proximity sensor ignore angle 5 + // @Description: Proximity sensor ignore angle 5 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity_Params, ignore_angle_deg[4], 0), + + // @Param: _IGN_WID5 + // @DisplayName: Proximity sensor ignore width 5 + // @Description: Proximity sensor ignore width 5 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity_Params, ignore_width_deg[4], 0), + + // @Param: _IGN_ANG6 + // @DisplayName: Proximity sensor ignore angle 6 + // @Description: Proximity sensor ignore angle 6 + // @Units: deg + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity_Params, ignore_angle_deg[5], 0), + + // @Param: _IGN_WID6 + // @DisplayName: Proximity sensor ignore width 6 + // @Description: Proximity sensor ignore width 6 + // @Units: deg + // @Range: 0 127 + // @User: Standard + AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity_Params, ignore_width_deg[5], 0), + + // @Param: _MIN + // @DisplayName: Proximity minimum range + // @Description: Minimum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range. + // @Units: m + // @Range: 0 500 + // @User: Advanced + AP_GROUPINFO("_MIN", 16, AP_Proximity_Params, min_m, 0.0f), + + // @Param: _MAX + // @DisplayName: Proximity maximum range + // @Description: Maximum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range. + // @Units: m + // @Range: 0 500 + // @User: Advanced + AP_GROUPINFO("_MAX", 17, AP_Proximity_Params, max_m, 0.0f), + + AP_GROUPEND +}; + +AP_Proximity_Params::AP_Proximity_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_Proximity/AP_Proximity_Params.h b/libraries/AP_Proximity/AP_Proximity_Params.h new file mode 100644 index 0000000000..c69b72adcd --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_Params.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include + +#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored + +class AP_Proximity_Params { + +public: + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Proximity_Params(void); + + /* Do not allow copies */ + AP_Proximity_Params(const AP_Proximity_Params &other) = delete; + AP_Proximity_Params &operator=(const AP_Proximity_Params&) = delete; + + AP_Int8 type; // type of sensor + AP_Int8 orientation; // orientation (e.g. right-side-up or upside-down) + AP_Int16 yaw_correction; // yaw correction in degrees + AP_Int16 ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up) + AP_Int8 ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored + AP_Float max_m; // maximum range in meters + AP_Float min_m; // minimum range in meters +}; diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 83310120c5..d67277249d 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -33,8 +33,9 @@ extern const AP_HAL::HAL& hal; The constructor also initialises the proximity sensor. */ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state): - AP_Proximity_Backend(_frontend, _state), + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params): + AP_Proximity_Backend(_frontend, _state, _params), sitl(AP::sitl()) { ap_var_type ptype; diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index c80c976c3b..ba85dd966d 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -14,7 +14,7 @@ class AP_Proximity_SITL : public AP_Proximity_Backend public: // constructor - AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params); // update state void update(void) override;