Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

SITL: add SIM_SONAR_ROT, use it for measuring horizontal distances

Browse Source
This commit is contained in:
Peter Barker
2022-04-19 10:26:10 +10:00
committed by Andrew Tridgell
parent 1340132f6f
commit 348f0ad590
5 changed files with 118 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/SITL/SIM_SerialProximitySensor.cpp
-94
View File
@@ -43,98 +43,4 @@ void SerialProximitySensor::update(const Location &location)
write_to_autopilot((char*)data, packetlen);
}
float SerialProximitySensor::measure_distance_at_angle_bf(const Location &location, float angle) const
{
const SIM *sitl = AP::sitl();
Vector2f vehicle_pos_cm;
if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) {
// should probably use SITL variables...
return 0.0f;
}
static uint64_t count = 0;
if (count == 0) {
unlink("/tmp/rayfile.scr");
unlink("/tmp/intersectionsfile.scr");
}
count++;
// the 1000 here is so the files don't grow unbounded
const bool write_debug_files = count < 1000;
FILE *rayfile = nullptr;
if (write_debug_files) {
rayfile = fopen("/tmp/rayfile.scr", "a");
}
// cast a ray from location out 200m...
Location location2 = location;
location2.offset_bearing(wrap_180(angle + sitl->state.yawDeg), 200);
Vector2f ray_endpos_cm;
if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) {
// should probably use SITL variables...
return 0.0f;
}
if (rayfile != nullptr) {
::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7);
fclose(rayfile);
}
// setup a grid of posts
FILE *postfile = nullptr;
FILE *intersectionsfile = nullptr;
if (write_debug_files) {
static bool postfile_written;
if (!postfile_written) {
::fprintf(stderr, "Writing /tmp/post-locations.scr\n");
postfile_written = true;
postfile = fopen("/tmp/post-locations.scr", "w");
}
intersectionsfile = fopen("/tmp/intersections.scr", "a");
}
const float radius_cm = 100.0f;
float min_dist_cm = 1000000.0;
const uint8_t num_post_offset = 10;
for (int8_t x=-num_post_offset; x<num_post_offset; x++) {
for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
Location post_location = post_origin;
post_location.offset(x*10+3, y*10+2);
if (postfile != nullptr) {
::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0);
}
Vector2f post_position_cm;
if (!post_location.get_vector_xy_from_origin_NE(post_position_cm)) {
// should probably use SITL variables...
return 0.0f;
}
Vector2f intersection_point_cm;
if (Vector2f::circle_segment_intersection(ray_endpos_cm, vehicle_pos_cm, post_position_cm, radius_cm, intersection_point_cm)) {
float dist_cm = (intersection_point_cm-vehicle_pos_cm).length();
if (intersectionsfile != nullptr) {
Location intersection_point = location;
intersection_point.offset(intersection_point_cm.x/100.0,
intersection_point_cm.y/100.0);
::fprintf(intersectionsfile,
"map icon %f %f barrell\n",
intersection_point.lat*1e-7,
intersection_point.lng*1e-7);
}
if (dist_cm < min_dist_cm) {
min_dist_cm = dist_cm;
}
}
}
}
if (postfile != nullptr) {
fclose(postfile);
}
if (intersectionsfile != nullptr) {
fclose(intersectionsfile);
}
// ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f);
return min_dist_cm / 100.0f;
}
#endif // HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED