Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: move params to separate class

Browse Source 
simplies increasing the maximum number of backends
This commit is contained in:
Randy Mackay 2022-07-05 16:28:24 +09:00 committed by Andrew Tridgell
parent c05b7271c4
commit 6025b1dcaa
12 changed files with 275 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -35,124 +35,10 @@ extern const AP_HAL::HAL &hal;
const AP_Param::GroupInfo AP_Proximity::var_info[] = { const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// 0 is reserved for possible addition of an ENABLED parameter // 0 is reserved for possible addition of an ENABLED parameter
// @Param: _TYPE // 1 was _TYPE
// @DisplayName: Proximity type // 2 was _ORIENT
// @Description: What type of proximity sensor is connected // 3 was _YAW_CORR
// @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1 // 4 to 15 was _IGN_ANG1 to _IGN_WID6
// @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),
// @Param{Copter}: _IGN_GND // @Param{Copter}: _IGN_GND
// @DisplayName: Proximity sensor land detection // @DisplayName: Proximity sensor land detection
@ -176,21 +62,30 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f), AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f),
// @Param: _MIN // 19 was _MIN
// @DisplayName: Proximity minimum range // 20 was _MAX
// @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),
// @Param: _MAX // @Group: 1
// @DisplayName: Proximity maximum range // @Path: AP_Proximity_Params.cpp
// @Description: Maximum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range. AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params),
// @Units: m
// @Range: 0 500 #if PROXIMITY_MAX_INSTANCES > 1
// @User: Advanced // @Group: 2
AP_GROUPINFO("_MAX", 20, AP_Proximity, _max_m, 0.0f), // @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 AP_GROUPEND
}; };
@ -253,7 +148,7 @@ uint8_t AP_Proximity::get_orientation(uint8_t instance) const
return 0; return 0;
} }
return _orientation[instance].get(); return params[instance].orientation.get();
} }
// return sensor yaw correction // return sensor yaw correction
@ -263,7 +158,7 @@ int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const
return 0; return 0;
} }
return _yaw_correction[instance].get(); return params[instance].yaw_correction.get();
} }
// return sensor health // 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. // detect if an instance of a proximity sensor is connected.
void AP_Proximity::detect_instance(uint8_t instance) void AP_Proximity::detect_instance(uint8_t instance)
{ {
@ -299,58 +207,58 @@ void AP_Proximity::detect_instance(uint8_t instance)
case Type::None: case Type::None:
return; return;
case Type::RPLidarA2: case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect()) { if (AP_Proximity_RPLidarA2::detect(instance)) {
state[instance].instance = 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; return;
} }
break; break;
case Type::MAV: case Type::MAV:
state[instance].instance = instance; 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; return;
case Type::TRTOWER: case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect()) { if (AP_Proximity_TeraRangerTower::detect(instance)) {
state[instance].instance = 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; return;
} }
break; break;
case Type::TRTOWEREVO: case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect()) { if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
state[instance].instance = 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; return;
} }
break; break;
case Type::RangeFinder: case Type::RangeFinder:
state[instance].instance = instance; 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; return;
case Type::SF40C: case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect()) { if (AP_Proximity_LightWareSF40C::detect(instance)) {
state[instance].instance = 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; return;
} }
break; break;
case Type::SF45B: case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect()) { if (AP_Proximity_LightWareSF45B::detect(instance)) {
state[instance].instance = 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; return;
} }
break; break;
case Type::CYGBOT_D1: case Type::CYGBOT_D1:
#if AP_PROXIMITY_CYGBOT_ENABLED #if AP_PROXIMITY_CYGBOT_ENABLED
if (AP_Proximity_Cygbot_D1::detect()) { if (AP_Proximity_Cygbot_D1::detect(instance)) {
state[instance].instance = 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; return;
} }
# endif # endif
@ -359,12 +267,12 @@ void AP_Proximity::detect_instance(uint8_t instance)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL: case Type::SITL:
state[instance].instance = instance; 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; return;
case Type::AirSimSITL: case Type::AirSimSITL:
state[instance].instance = instance; 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; return;
#endif #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 AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{ {
if (instance < PROXIMITY_MAX_INSTANCES) { if (instance < PROXIMITY_MAX_INSTANCES) {
return (Type)((uint8_t)_type[instance]); return (Type)((uint8_t)params[instance].type);
} }
return Type::None; return Type::None;
} }

33
libraries/AP_Proximity/AP_Proximity.h
View File

@ -22,9 +22,9 @@
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Proximity_Params.h"
#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_INSTANCES 2 // 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_DIRECTION 8 #define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10 #define PROXIMITY_SENSOR_ID_START 10
@ -164,6 +164,11 @@ public:
// messages: // messages:
void log(); void log();
protected:
// parameters for backends
AP_Proximity_Params params[PROXIMITY_MAX_INSTANCES];
private: private:
static AP_Proximity *_singleton; static AP_Proximity *_singleton;
Proximity_State state[PROXIMITY_MAX_INSTANCES]; Proximity_State state[PROXIMITY_MAX_INSTANCES];
@ -171,27 +176,15 @@ private:
uint8_t primary_instance; uint8_t primary_instance;
uint8_t num_instances; uint8_t num_instances;
bool valid_instance(uint8_t i) const { // return true if the given instance exists
if (drivers[i] == nullptr) { bool valid_instance(uint8_t i) const;
return false;
}
return (Type)_type[i].get() != Type::None;
}
// 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); void detect_instance(uint8_t instance);
// parameters for all instances
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
}; };
namespace AP { namespace AP {

17
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -28,9 +28,10 @@ extern const AP_HAL::HAL& hal;
base class constructor. base class constructor.
This incorporates initialisation as well. 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), 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 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 // check if distances are supposed to be in a particular range
if (!is_zero(frontend._max_m)) { if (!is_zero(params.max_m)) {
if (distance_m > frontend._max_m) { if (distance_m > params.max_m) {
// too far away // too far away
return true; return true;
} }
} }
if (!is_zero(frontend._min_m)) { if (!is_zero(params.min_m)) {
if (distance_m < frontend._min_m) { if (distance_m < params.min_m) {
// too close // too close
return true; return true;
} }
@ -102,8 +103,8 @@ bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance
if (check_for_ign_area) { if (check_for_ign_area) {
// check angle vs each ignore area // check angle vs each ignore area
for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) { for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
if (frontend._ignore_width_deg[i] != 0) { if (params.ignore_width_deg[i] != 0) {
if (abs(yaw - frontend._ignore_angle_deg[i]) <= (frontend._ignore_width_deg[i]/2)) { if (abs(yaw - params.ignore_angle_deg[i]) <= (params.ignore_width_deg[i]/2)) {
return true; return true;
} }
} }

3
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -28,7 +28,7 @@ class AP_Proximity_Backend
{ {
public: public:
// constructor. This incorporates initialisation as well. // 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 // we declare a virtual destructor so that Proximity drivers can
// override with a custom destructor if need be // override with a custom destructor if need be
@ -121,6 +121,7 @@ protected:
AP_Proximity &frontend; AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state AP_Proximity::Proximity_State &state; // reference to this instances state
AP_Proximity_Params &params; // parameters for this backend
// Methods to manipulate 3D boundary in this class // Methods to manipulate 3D boundary in this class
AP_Proximity_Boundary_3D boundary; AP_Proximity_Boundary_3D boundary;

9
libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
View File

@ -24,8 +24,9 @@
already know that we should setup the proximity sensor already know that we should setup the proximity sensor
*/ */
AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend, AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state) : AP_Proximity::Proximity_State &_state,
AP_Proximity_Backend(_frontend, _state) AP_Proximity_Params &_params) :
AP_Proximity_Backend(_frontend, _state, _params)
{ {
const AP_SerialManager &serial_manager = AP::serialmanager(); const AP_SerialManager &serial_manager = AP::serialmanager();
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); _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 // detect if a proximity sensor is connected by looking for a
// configured serial port // 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 #endif // HAL_PROXIMITY_ENABLED

6
libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
View File

@ -8,9 +8,11 @@ class AP_Proximity_Backend_Serial : public AP_Proximity_Backend
{ {
public: public:
AP_Proximity_Backend_Serial(AP_Proximity &_frontend, 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 detection function
static bool detect(); static bool detect(uint8_t instance);
protected: protected:
virtual uint16_t rxspace() const { return 0; }; virtual uint16_t rxspace() const { return 0; };

2
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

@ -164,7 +164,7 @@ private:
}; };
// This class gives an easy way of making a temporary boundary, used for "sorting" distances. // 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. // 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 class AP_Proximity_Temp_Boundary
{ {

5
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
View File

@ -11,8 +11,9 @@ class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial
public: public:
// constructor // constructor
AP_Proximity_LightWareSF45B(AP_Proximity &_frontend, AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state) : AP_Proximity::Proximity_State &_state,
AP_Proximity_LightWareSerial(_frontend, _state) {} AP_Proximity_Params& _params) :
AP_Proximity_LightWareSerial(_frontend, _state, _params) {}
uint16_t rxspace() const override { uint16_t rxspace() const override {
return 1280; return 1280;

148
libraries/AP_Proximity/AP_Proximity_Params.cpp Normal file
View File

@ -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);
}

27
libraries/AP_Proximity/AP_Proximity_Params.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#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
};

5
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -33,8 +33,9 @@ extern const AP_HAL::HAL& hal;
The constructor also initialises the proximity sensor. The constructor also initialises the proximity sensor.
*/ */
AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state): AP_Proximity::Proximity_State &_state,
AP_Proximity_Backend(_frontend, _state), AP_Proximity_Params& _params):
AP_Proximity_Backend(_frontend, _state, _params),
sitl(AP::sitl()) sitl(AP::sitl())
{ {
ap_var_type ptype; ap_var_type ptype;

2
libraries/AP_Proximity/AP_Proximity_SITL.h
View File

@ -14,7 +14,7 @@ class AP_Proximity_SITL : public AP_Proximity_Backend
public: public:
// constructor // 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 // update state
void update(void) override; void update(void) override;