diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 306751e617..f052965c6f 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -215,7 +215,7 @@ void AP_Proximity::update(void) { for (uint8_t i=0; ihandle_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. void AP_Proximity::detect_instance(uint8_t instance) { - uint8_t type = _type[instance]; - if (type == Proximity_Type_SF40C) { + switch (get_type(instance)) { + case Type::None: + return; + case Type::SF40C: if (AP_Proximity_LightWareSF40C::detect()) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance]); return; } - } - if (type == Proximity_Type_RPLidarA2) { + break; + case Type::RPLidarA2: if (AP_Proximity_RPLidarA2::detect()) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]); return; } - } - if (type == Proximity_Type_MAV) { + break; + case Type::MAV: state[instance].instance = instance; drivers[instance] = new AP_Proximity_MAV(*this, state[instance]); return; - } - if (type == Proximity_Type_TRTOWER) { + + case Type::TRTOWER: if (AP_Proximity_TeraRangerTower::detect()) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance]); return; } - } - if (type == Proximity_Type_TRTOWEREVO) { + break; + case Type::TRTOWEREVO: if (AP_Proximity_TeraRangerTowerEvo::detect()) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance]); return; } - } - if (type == Proximity_Type_RangeFinder) { + break; + + case Type::RangeFinder: state[instance].instance = instance; drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]); return; - } + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - if (type == Proximity_Type_SITL) { + case Type::SITL: state[instance].instance = instance; drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); return; - } - if (type == Proximity_Type_MorseSITL) { + + case Type::MorseSITL: state[instance].instance = instance; drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]); return; - } - if (type == Proximity_Type_AirSimSITL) { + + case Type::AirSimSITL: state[instance].instance = instance; drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]); return; - } #endif + } } // get distance in meters in a particular direction in degrees (0 is forward, clockwise) // 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 { - if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { + if (!valid_instance(instance)) { return false; } // 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 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; } // 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 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; 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 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; } // 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 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; } // 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 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; } // 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 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; } // get maximum distance from backend @@ -427,7 +430,7 @@ float AP_Proximity::distance_max() 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; } // get minimum distance from backend @@ -437,7 +440,7 @@ float AP_Proximity::distance_min() const // get distance in meters upwards, returns true on success 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; } // 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); } -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) { - 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 @@ -463,7 +466,7 @@ bool AP_Proximity::sensor_present() 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 { diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 2ead768cfe..f0195e6e5b 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -38,17 +38,19 @@ public: AP_Proximity &operator=(const AP_Proximity) = delete; // Proximity driver types - enum Proximity_Type { - Proximity_Type_None = 0, - Proximity_Type_SF40C = 1, - Proximity_Type_MAV = 2, - Proximity_Type_TRTOWER = 3, - Proximity_Type_RangeFinder = 4, - Proximity_Type_RPLidarA2 = 5, - Proximity_Type_TRTOWEREVO = 6, - Proximity_Type_SITL = 10, - Proximity_Type_MorseSITL = 11, - Proximity_Type_AirSimSITL = 12, + enum class Type { + None = 0, + SF40C = 1, + MAV = 2, + TRTOWER = 3, + RangeFinder = 4, + RPLidarA2 = 5, + TRTOWEREVO = 6, +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL = 10, + MorseSITL = 11, + AirSimSITL = 12, +#endif }; enum Proximity_Status { @@ -128,7 +130,7 @@ public: bool get_upward_distance(uint8_t instance, 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 static const struct AP_Param::GroupInfo var_info[]; @@ -149,6 +151,13 @@ 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];