zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Proximity: reorder method declarations and implementations

master
Randy Mackay 3 years ago committed by Andrew Tridgell
parent
b6d133e285
commit
eff86c88ab
2 changed files with 193 additions and 182 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 281
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 94
      libraries/AP_Proximity/AP_Proximity.h

281
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -103,7 +103,7 @@ AP_Proximity::AP_Proximity() @@ -103,7 +103,7 @@ AP_Proximity::AP_Proximity()
// initialise the Proximity class. We do detection of attached sensors here
// we don't allow for hot-plugging of sensors (i.e. reboot required)
void AP_Proximity::init(void)
void AP_Proximity::init()
{
if (num_instances != 0) {
// init called a 2nd time?
@ -123,7 +123,7 @@ void AP_Proximity::init(void) @@ -123,7 +123,7 @@ void AP_Proximity::init(void)
}
// update Proximity state for all instances. This should be called at a high rate by the main loop
void AP_Proximity::update(void)
void AP_Proximity::update()
{
for (uint8_t i=0; i<num_instances; i++) {
if (!valid_instance(i)) {
@ -141,6 +141,14 @@ void AP_Proximity::update(void) @@ -141,6 +141,14 @@ void AP_Proximity::update(void)
}
}
AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{
if (instance < PROXIMITY_MAX_INSTANCES) {
return (Type)((uint8_t)params[instance].type);
}
return Type::None;
}
// return sensor orientation
uint8_t AP_Proximity::get_orientation(uint8_t instance) const
{
@ -177,107 +185,24 @@ AP_Proximity::Status AP_Proximity::get_status() const @@ -177,107 +185,24 @@ AP_Proximity::Status AP_Proximity::get_status() const
return get_status(primary_instance);
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity::handle_msg(const mavlink_message_t &msg)
// get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity::distance_max() const
{
for (uint8_t i=0; i<num_instances; i++) {
if (valid_instance(i)) {
drivers[i]->handle_msg(msg);
}
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get maximum distance from backend
return drivers[primary_instance]->distance_max();
}
// return true if the given instance exists
bool AP_Proximity::valid_instance(uint8_t i) const
float AP_Proximity::distance_min() const
{
if (i >= PROXIMITY_MAX_INSTANCES) {
return false;
}
if (drivers[i] == nullptr) {
return false;
if (!valid_instance(primary_instance)) {
return 0.0f;
}
return (Type)params[i].type.get() != Type::None;
// get minimum distance from backend
return drivers[primary_instance]->distance_min();
}
// detect if an instance of a proximity sensor is connected.
void AP_Proximity::detect_instance(uint8_t instance)
{
switch (get_type(instance)) {
case Type::None:
return;
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect(instance)) {
state[instance].instance = 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], params[instance]);
return;
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
return;
}
break;
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
state[instance].instance = 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], params[instance]);
return;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
return;
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect(instance)) {
state[instance].instance = 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(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
return;
}
# endif
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = 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], params[instance]);
return;
#endif
}
}
// get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
@ -289,6 +214,15 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a @@ -289,6 +214,15 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
}
// get number of layers.
uint8_t AP_Proximity::get_num_layers() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_num_layers();
}
// get raw and filtered distances in 8 directions per layer. used for logging
bool AP_Proximity::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
{
@ -308,15 +242,6 @@ uint8_t AP_Proximity::get_obstacle_count() const @@ -308,15 +242,6 @@ uint8_t AP_Proximity::get_obstacle_count() const
return drivers[primary_instance]->get_obstacle_count();
}
// get number of layers.
uint8_t AP_Proximity::get_num_layers() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_num_layers();
}
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{
@ -368,22 +293,29 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a @@ -368,22 +293,29 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
}
// get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity::distance_max() const
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity::handle_msg(const mavlink_message_t &msg)
{
if (!valid_instance(primary_instance)) {
return 0.0f;
for (uint8_t i=0; i<num_instances; i++) {
if (valid_instance(i)) {
drivers[i]->handle_msg(msg);
}
}
// get maximum distance from backend
return drivers[primary_instance]->distance_max();
}
float AP_Proximity::distance_min() const
// methods for mavlink SYS_STATUS message (send_sys_status)
// these methods cover only the primary instance
bool AP_Proximity::sensor_present() const
{
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get minimum distance from backend
return drivers[primary_instance]->distance_min();
return get_status() != Status::NotConnected;
}
bool AP_Proximity::sensor_enabled() const
{
return get_type(primary_instance) != Type::None;
}
bool AP_Proximity::sensor_failed() const
{
return get_status() != Status::Good;
}
// get distance in meters upwards, returns true on success
@ -401,27 +333,6 @@ bool AP_Proximity::get_upward_distance(float &distance) const @@ -401,27 +333,6 @@ bool AP_Proximity::get_upward_distance(float &distance) const
return get_upward_distance(primary_instance, distance);
}
AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{
if (instance < PROXIMITY_MAX_INSTANCES) {
return (Type)((uint8_t)params[instance].type);
}
return Type::None;
}
bool AP_Proximity::sensor_present() const
{
return get_status() != Status::NotConnected;
}
bool AP_Proximity::sensor_enabled() const
{
return get_type(primary_instance) != Type::None;
}
bool AP_Proximity::sensor_failed() const
{
return get_status() != Status::Good;
}
// set alt as read from dowward facing rangefinder. Tilt is already adjusted for.
void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
{
@ -498,6 +409,100 @@ void AP_Proximity::log() @@ -498,6 +409,100 @@ void AP_Proximity::log()
}
#endif
// 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)
{
switch (get_type(instance)) {
case Type::None:
return;
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect(instance)) {
state[instance].instance = 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], params[instance]);
return;
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
return;
}
break;
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
state[instance].instance = 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], params[instance]);
return;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
return;
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect(instance)) {
state[instance].instance = 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(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
return;
}
# endif
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = 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], params[instance]);
return;
#endif
}
}
AP_Proximity *AP_Proximity::_singleton;
namespace AP {

94
libraries/AP_Proximity/AP_Proximity.h
Unescape View File

@ -64,23 +64,17 @@ public: @@ -64,23 +64,17 @@ public:
Good
};
// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
// detect and initialise any available proximity sensors
void init(void);
void init();
// update state of all proximity sensors. Should be called at high rate from main loop
void update(void);
void update();
// return the number of proximity sensor backends
uint8_t num_sensors() const { return num_instances; }
// return sensor type of a given instance
Type get_type(uint8_t instance) const;
// return sensor orientation and yaw correction
uint8_t get_orientation(uint8_t instance) const;
@ -91,14 +85,32 @@ public: @@ -91,14 +85,32 @@ public:
Status get_status(uint8_t instance) const;
Status get_status() const;
// Return the number of proximity sensors
uint8_t num_sensors(void) const {
return num_instances;
}
// get maximum and minimum distances (in meters) of primary sensor
float distance_max() const;
float distance_min() const;
//
// 3D boundary related methods
//
// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get number of layers in the 3D boundary
uint8_t get_num_layers() const;
// get raw and filtered distances in 8 directions per layer. used for logging
bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
@ -120,50 +132,44 @@ public: @@ -120,50 +132,44 @@ public:
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// get number of layers
uint8_t get_num_layers() const;
// get maximum and minimum distances (in meters) of primary sensor
float distance_max() const;
float distance_min() const;
//
// mavlink related methods
//
// handle mavlink DISTANCE_SENSOR messages
void handle_msg(const mavlink_message_t &msg);
// The Proximity_State structure is filled in by the backend driver
struct Proximity_State {
uint8_t instance; // the instance number of this proximity sensor
Status status; // sensor status
};
// methods for mavlink SYS_STATUS message (send_sys_status)
// these methods cover only the primary instance
bool sensor_present() const;
bool sensor_enabled() const;
bool sensor_failed() const;
//
// support for upward facing sensors
// support for upwards and downwards facing sensors
//
// get distance upwards in meters. returns true on success
bool get_upward_distance(uint8_t instance, float &distance) const;
bool get_upward_distance(float &distance) const;
Type get_type(uint8_t instance) const;
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
// method called by vehicle to have AP_Proximity write onboard log messages
void log();
// The Proximity_State structure is filled in by the backend driver
struct Proximity_State {
uint8_t instance; // the instance number of this proximity sensor
Status status; // sensor status
};
// parameter list
static const struct AP_Param::GroupInfo var_info[];
static AP_Proximity *get_singleton(void) { return _singleton; };
// methods for mavlink SYS_STATUS message (send_sys_status)
// these methods cover only the primary instance
bool sensor_present() const;
bool sensor_enabled() const;
bool sensor_failed() const;
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
// method called by vehicle to have AP_Proximity write onboard log
// messages:
void log();
protected:
// parameters for backends

Write Preview
Loading…
Cancel
Save