aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: use enum class for proximity type

Browse Source 
Removes verbiage from AP_Proximity.cpp
This commit is contained in:
Peter Barker 2019-09-27 18:43:39 +10:00 committed by Randy Mackay
parent 30399942c1
commit ee0dd26007
2 changed files with 58 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -215,7 +215,7 @@ void AP_Proximity::update(void)
{ {
for (uint8_t i=0; i<num_instances; i++) { for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != nullptr) { if (drivers[i] != nullptr) {
if (_type[i] == Proximity_Type_None) { if (get_type(i) == Type::None) {
// allow user to disable a proximity sensor at runtime // allow user to disable a proximity sensor at runtime
state[i].status = Proximity_NotConnected; state[i].status = Proximity_NotConnected;
continue; continue;
@ -272,7 +272,7 @@ AP_Proximity::Proximity_Status AP_Proximity::get_status() const
void AP_Proximity::handle_msg(const mavlink_message_t &msg) void AP_Proximity::handle_msg(const mavlink_message_t &msg)
{ {
for (uint8_t i=0; i<num_instances; i++) { for (uint8_t i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != Proximity_Type_None)) { if (valid_instance(i)) {
drivers[i]->handle_msg(msg); drivers[i]->handle_msg(msg);
} }
} }
@ -281,69 +281,72 @@ void AP_Proximity::handle_msg(const mavlink_message_t &msg)
// 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)
{ {
uint8_t type = _type[instance]; switch (get_type(instance)) {
if (type == Proximity_Type_SF40C) { case Type::None:
return;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect()) { if (AP_Proximity_LightWareSF40C::detect()) {
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]);
return; return;
} }
} break;
if (type == Proximity_Type_RPLidarA2) { case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect()) { if (AP_Proximity_RPLidarA2::detect()) {
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]);
return; return;
} }
} break;
if (type == Proximity_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]);
return; return;
}
if (type == Proximity_Type_TRTOWER) { case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect()) { if (AP_Proximity_TeraRangerTower::detect()) {
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]);
return; return;
} }
} break;
if (type == Proximity_Type_TRTOWEREVO) { case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect()) { if (AP_Proximity_TeraRangerTowerEvo::detect()) {
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]);
return; return;
} }
} break;
if (type == Proximity_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]);
return; return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type == Proximity_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]);
return; return;
}
if (type == Proximity_Type_MorseSITL) { case Type::MorseSITL:
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]); drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]);
return; return;
}
if (type == Proximity_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]);
return; return;
}
#endif #endif
}
} }
// get distance in meters in a particular direction in degrees (0 is forward, clockwise) // get distance in meters in a particular direction in degrees (0 is forward, clockwise)
// returns true on successful read and places distance in distance // returns true on successful read and places distance in distance
bool AP_Proximity::get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const bool AP_Proximity::get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const
{ {
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { if (!valid_instance(instance)) {
return false; return false;
} }
// get distance from backend // get distance from backend
@ -360,7 +363,7 @@ bool AP_Proximity::get_horizontal_distance(float angle_deg, float &distance) con
// get distances in 8 directions. used for sending distances to ground station // 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 bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return false; return false;
} }
// get distances from backend // get distances from backend
@ -371,7 +374,7 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
// returns nullptr and sets num_points to zero if no boundary can be returned // returns nullptr and sets num_points to zero if no boundary can be returned
const Vector2f* AP_Proximity::get_boundary_points(uint8_t instance, uint16_t& num_points) const const Vector2f* AP_Proximity::get_boundary_points(uint8_t instance, uint16_t& num_points) const
{ {
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { if (!valid_instance(instance)) {
num_points = 0; num_points = 0;
return nullptr; return nullptr;
} }
@ -388,7 +391,7 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
// returns true on success, false if no valid readings // returns true on success, false if no valid readings
bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return false; return false;
} }
// get closest object from backend // get closest object from backend
@ -398,7 +401,7 @@ bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
// get number of objects, used for non-GPS avoidance // get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity::get_object_count() const uint8_t AP_Proximity::get_object_count() const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return 0; return 0;
} }
// get count from backend // get count from backend
@ -409,7 +412,7 @@ uint8_t AP_Proximity::get_object_count() const
// returns false if no angle or distance could be returned for some reason // returns false if no angle or distance could be returned for some reason
bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return false; return false;
} }
// get angle and distance from backend // get angle and distance from backend
@ -419,7 +422,7 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a
// get maximum and minimum distances (in meters) of primary sensor // get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity::distance_max() const float AP_Proximity::distance_max() const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return 0.0f; return 0.0f;
} }
// get maximum distance from backend // get maximum distance from backend
@ -427,7 +430,7 @@ float AP_Proximity::distance_max() const
} }
float AP_Proximity::distance_min() const float AP_Proximity::distance_min() const
{ {
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) { if (!valid_instance(primary_instance)) {
return 0.0f; return 0.0f;
} }
// get minimum distance from backend // get minimum distance from backend
@ -437,7 +440,7 @@ float AP_Proximity::distance_min() const
// get distance in meters upwards, returns true on success // get distance in meters upwards, returns true on success
bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const
{ {
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { if (!valid_instance(instance)) {
return false; return false;
} }
// get upward distance from backend // get upward distance from backend
@ -449,12 +452,12 @@ bool AP_Proximity::get_upward_distance(float &distance) const
return get_upward_distance(primary_instance, distance); return get_upward_distance(primary_instance, distance);
} }
AP_Proximity::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 (Proximity_Type)((uint8_t)_type[instance]); return (Type)((uint8_t)_type[instance]);
} }
return Proximity_Type_None; return Type::None;
} }
bool AP_Proximity::sensor_present() const bool AP_Proximity::sensor_present() const
@ -463,7 +466,7 @@ bool AP_Proximity::sensor_present() const
} }
bool AP_Proximity::sensor_enabled() const bool AP_Proximity::sensor_enabled() const
{ {
return _type[primary_instance] != Proximity_Type_None; return get_type(primary_instance) != Type::None;
} }
bool AP_Proximity::sensor_failed() const bool AP_Proximity::sensor_failed() const
{ {

33
libraries/AP_Proximity/AP_Proximity.h
View File

@ -38,17 +38,19 @@ public:
AP_Proximity &operator=(const AP_Proximity) = delete; AP_Proximity &operator=(const AP_Proximity) = delete;
// Proximity driver types // Proximity driver types
enum Proximity_Type { enum class Type {
Proximity_Type_None = 0, None = 0,
Proximity_Type_SF40C = 1, SF40C = 1,
Proximity_Type_MAV = 2, MAV = 2,
Proximity_Type_TRTOWER = 3, TRTOWER = 3,
Proximity_Type_RangeFinder = 4, RangeFinder = 4,
Proximity_Type_RPLidarA2 = 5, RPLidarA2 = 5,
Proximity_Type_TRTOWEREVO = 6, TRTOWEREVO = 6,
Proximity_Type_SITL = 10, #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
Proximity_Type_MorseSITL = 11, SITL = 10,
Proximity_Type_AirSimSITL = 12, MorseSITL = 11,
AirSimSITL = 12,
#endif
}; };
enum Proximity_Status { enum Proximity_Status {
@ -128,7 +130,7 @@ public:
bool get_upward_distance(uint8_t instance, float &distance) const; bool get_upward_distance(uint8_t instance, float &distance) const;
bool get_upward_distance(float &distance) const; bool get_upward_distance(float &distance) const;
Proximity_Type get_type(uint8_t instance) const; Type get_type(uint8_t instance) const;
// parameter list // parameter list
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -149,6 +151,13 @@ private:
uint8_t primary_instance; uint8_t primary_instance;
uint8_t num_instances; 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 // parameters for all instances
AP_Int8 _type[PROXIMITY_MAX_INSTANCES]; AP_Int8 _type[PROXIMITY_MAX_INSTANCES];
AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES]; AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES];