diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index dd2cac8837..6a68d2c5fe 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -21,6 +21,7 @@ #include "AP_Proximity_RangeFinder.h" #include "AP_Proximity_MAV.h" #include "AP_Proximity_SITL.h" +#include "AP_Proximity_MorseSITL.h" extern const AP_HAL::HAL &hal; @@ -31,7 +32,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,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo + // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,10:SITL,11:MorseSITL // @RebootRequired: True // @User: Standard AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0), @@ -324,6 +325,11 @@ void AP_Proximity::detect_instance(uint8_t instance) drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); return; } + if (type == Proximity_Type_MorseSITL) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]); + return; + } #endif } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index bfa330b5ef..84c6987a1e 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -49,6 +49,7 @@ public: Proximity_Type_RPLidarA2 = 5, Proximity_Type_TRTOWEREVO = 6, Proximity_Type_SITL = 10, + Proximity_Type_MorseSITL = 11, }; enum Proximity_Status { diff --git a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp new file mode 100644 index 0000000000..6d4404adf6 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp @@ -0,0 +1,104 @@ +/* + 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 + +/* + The constructor also initialises the proximity sensor. +*/ +AP_Proximity_MorseSITL::AP_Proximity_MorseSITL(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state): + AP_Proximity_Backend(_frontend, _state), + sitl(AP::sitl()) +{ +} + +// 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::Proximity_NoData); + return; + } + + set_status(AP_Proximity::Proximity_Good); + + memset(_distance_valid, 0, sizeof(_distance_valid)); + memset(_angle, 0, sizeof(_angle)); + memset(_distance, 0, sizeof(_distance)); + + // only use 8 sectors to match RPLidar + const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX); + const uint16_t degrees_per_sector = 360 / nsectors; + + for (uint16_t i=0; i + +class AP_Proximity_MorseSITL : public AP_Proximity_Backend +{ + +public: + // constructor + AP_Proximity_MorseSITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + + // 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; + float distance_maximum; +}; +#endif // CONFIG_HAL_BOARD