mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Proximity: move params to separate class
simplies increasing the maximum number of backends
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
c05b7271c4
commit
6025b1dcaa
@ -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;
|
||||
}
|
||||
|
||||
@ -22,9 +22,9 @@
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.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_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;
|
||||
}
|
||||
|
||||
// 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
|
||||
// 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 _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 {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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; };
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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;
|
||||
|
||||
148
libraries/AP_Proximity/AP_Proximity_Params.cpp
Normal file
148
libraries/AP_Proximity/AP_Proximity_Params.cpp
Normal 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
27
libraries/AP_Proximity/AP_Proximity_Params.h
Normal 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
|
||||
};
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user