diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index eae429b345..fdd35644f0 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -23,7 +23,6 @@ #include "AP_Proximity_LightWareSF40C.h" #include "AP_Proximity_LightWareSF45B.h" #include "AP_Proximity_SITL.h" -#include "AP_Proximity_MorseSITL.h" #include "AP_Proximity_AirSimSITL.h" extern const AP_HAL::HAL &hal; @@ -35,7 +34,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,11:MorseSITL,12:AirSimSITL + // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL // @RebootRequired: True // @User: Standard AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0), @@ -155,7 +154,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Param: 2_TYPE // @DisplayName: Second Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,11:MorseSITL,12:AirSimSITL + // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL // @User: Advanced // @RebootRequired: True AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0), @@ -342,11 +341,6 @@ void AP_Proximity::detect_instance(uint8_t instance) drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); return; - case Type::MorseSITL: - state[instance].instance = instance; - drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]); - return; - case Type::AirSimSITL: state[instance].instance = instance; drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]); diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 18976ba9bd..c5b32b799f 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -50,7 +50,6 @@ public: SF45B = 8, #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL = 10, - MorseSITL = 11, AirSimSITL = 12, #endif }; diff --git a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp deleted file mode 100644 index 2740360e55..0000000000 --- a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include "AP_Proximity_MorseSITL.h" -#include - -extern const AP_HAL::HAL& hal; - -#define PROXIMITY_MAX_RANGE 200.0f -#define PROXIMITY_ACCURACY 0.1f - -// update the state of the sensor -void AP_Proximity_MorseSITL::update(void) -{ - SITL::vector3f_array &points = sitl->state.scanner.points; - SITL::float_array &ranges = sitl->state.scanner.ranges; - if (points.length != ranges.length || - points.length == 0) { - set_status(AP_Proximity::Status::NoData); - return; - } - - set_status(AP_Proximity::Status::Good); - - memset(_distance_valid, 0, sizeof(_distance_valid)); - memset(_angle, 0, sizeof(_angle)); - memset(_distance, 0, sizeof(_distance)); - - for (uint16_t i=0; i - -class AP_Proximity_MorseSITL : public AP_Proximity_Backend -{ - -public: - // constructor - using AP_Proximity_Backend::AP_Proximity_Backend; - - // update state - void update(void) override; - - // get maximum and minimum distances (in meters) of sensor - float distance_max() const override; - float distance_min() const override; - - // get distance upwards in meters. returns true on success - bool get_upward_distance(float &distance) const override; - -private: - SITL::SITL *sitl = AP::sitl(); - float distance_maximum; -}; -#endif // CONFIG_HAL_BOARD