From 16a9506cf204fdecbef679e1bf048cffdc6a11d9 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 18 Jul 2020 00:37:34 +0100 Subject: [PATCH] SITL: SIM_Aircraft: add rangefinder array --- libraries/SITL/SIM_Aircraft.cpp | 8 ++++++++ libraries/SITL/SIM_Aircraft.h | 5 ++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index e5ace777e4..4e2badda83 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -83,6 +83,11 @@ Aircraft::Aircraft(const char *frame_str) : } terrain = &AP::terrain(); + + // init rangefinder array to -1 to signify no data + for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++){ + rangefinder_m[i] = -1.0f; + } } void Aircraft::set_start_location(const Location &start_loc, const float start_yaw) @@ -374,6 +379,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.scanner.points = scanner.points; fdm.scanner.ranges = scanner.ranges; + // copy rangefinder + memcpy(fdm.rangefinder_m, rangefinder_m, sizeof(fdm.rangefinder_m)); + if (is_smoothed) { fdm.xAccel = smoothing.accel_body.x; fdm.yAccel = smoothing.accel_body.y; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index b9adea4acc..c3db80ee0a 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -181,7 +181,10 @@ protected: struct vector3f_array points; struct float_array ranges; } scanner; - + + // Rangefinder + float rangefinder_m[RANGEFINDER_MAX_INSTANCES]; + // Wind Turbulence simulated Data float turbulence_azimuth = 0.0f; float turbulence_horizontal_speed = 0.0f; // m/s