diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 572bcec3b9..f5a11db804 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -15,6 +15,7 @@ #include "AP_Proximity.h" #include "AP_Proximity_LightWareSF40C.h" +#include "AP_Proximity_SITL.h" extern const AP_HAL::HAL &hal; @@ -167,6 +168,13 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } } +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (type == Proximity_Type_SITL) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); + return; + } +#endif } // get distance in meters in a particular direction in degrees (0 is forward, clockwise) diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index c31e2677c4..7984051226 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -37,6 +37,7 @@ public: enum Proximity_Type { Proximity_Type_None = 0, Proximity_Type_SF40C = 1, + Proximity_Type_SITL = 10, }; enum Proximity_Status { diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp new file mode 100644 index 0000000000..6c6d9f059d --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -0,0 +1,105 @@ +/* + 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 +#include "AP_Proximity_SITL.h" +#include + +extern const AP_HAL::HAL& hal; + +#define PROXIMITY_MAX_RANGE 200 +#define PROXIMITY_ACCURACY 0.1 + +/* + The constructor also initialises the proximity sensor. +*/ +AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state): + AP_Proximity_Backend(_frontend, _state) +{ + sitl = (SITL::SITL *)AP_Param::find_object("SIM_"); + ap_var_type ptype; + fence_count = (AP_Int8 *)AP_Param::find("FENCE_TOTAL", &ptype); + if (fence_count == nullptr || ptype != AP_PARAM_INT8) { + AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL"); + } +} + +// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction) +bool AP_Proximity_SITL::get_horizontal_distance(float angle_deg, float &distance) const +{ + if (!fence_loader.boundary_valid(fence_count->get(), fence, true)) { + return false; + } + + // convert to earth frame + angle_deg = wrap_360(sitl->state.yawDeg + angle_deg); + + /* + simple bisection search to find distance. Not really efficient, + but we can afford the CPU in SITL + */ + float min_dist = 0, max_dist = PROXIMITY_MAX_RANGE; + while (max_dist - min_dist > PROXIMITY_ACCURACY) { + float test_dist = (max_dist+min_dist)*0.5f; + Location loc = current_loc; + location_update(loc, angle_deg, test_dist); + Vector2l vecloc(loc.lat, loc.lng); + if (fence_loader.boundary_breached(vecloc, fence_count->get(), fence, true)) { + max_dist = test_dist; + } else { + min_dist = test_dist; + } + } + distance = min_dist; + return true; +} + +// update the state of the sensor +void AP_Proximity_SITL::update(void) +{ + load_fence(); + current_loc.lat = sitl->state.latitude * 1.0e7; + current_loc.lng = sitl->state.longitude * 1.0e7; + current_loc.alt = sitl->state.altitude * 1.0e2; + if (fence && fence_loader.boundary_valid(fence_count->get(), fence, true)) { + set_status(AP_Proximity::Proximity_Good); + } else { + set_status(AP_Proximity::Proximity_NoData); + } +} + +void AP_Proximity_SITL::load_fence(void) +{ + uint32_t now = AP_HAL::millis(); + if (now - last_load_ms < 1000) { + return; + } + last_load_ms = now; + + if (fence == nullptr) { + fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l)); + } + if (fence == nullptr) { + return; + } + for (uint8_t i=0; iget(); i++) { + fence_loader.load_point_from_eeprom(i, fence[i]); + } +} +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h new file mode 100644 index 0000000000..c21c42aedf --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -0,0 +1,33 @@ +#pragma once + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#include + +class AP_Proximity_SITL : public AP_Proximity_Backend +{ + +public: + // constructor + AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + + // 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 get_horizontal_distance(float angle_deg, float &distance) const override; + + // update state + void update(void) override; + +private: + SITL::SITL *sitl; + Vector2l *fence; + AP_Int8 *fence_count; + uint32_t last_load_ms; + AC_PolyFence_loader fence_loader; + Location current_loc; + + void load_fence(void); +}; +#endif // CONFIG_HAL_BOARD