From e2fbdb8823cbd66660ee1f3c51da8c2c7b527486 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 21 Aug 2023 10:51:13 +1000
Subject: [PATCH] SITL: use NaN for invalid rangefinder data

needed to cope properly with terrain errors leading to negative
rangefinder data
---
 libraries/SITL/SIM_Aircraft.cpp       | 4 ++--
 libraries/SITL/SIM_DroneCANDevice.cpp | 4 ++--
 libraries/SITL/SIM_FlightAxis.cpp     | 2 +-
 libraries/SITL/SIM_Frame.cpp          | 3 ++-
 libraries/SITL/SITL.cpp               | 2 +-
 5 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 154145c48982d2..f2620a8460e741 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -71,9 +71,9 @@ Aircraft::Aircraft(const char *frame_str) :
         sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
     }
 
-    // init rangefinder array to -1 to signify no data
+    // init rangefinder array to NaN to signify no data
     for (uint8_t i = 0; i < ARRAY_SIZE(rangefinder_m); i++){
-        rangefinder_m[i] = -1.0f;
+        rangefinder_m[i] = nanf("");
     }
 }
 
diff --git a/libraries/SITL/SIM_DroneCANDevice.cpp b/libraries/SITL/SIM_DroneCANDevice.cpp
index 51d1c9ac3d7340..598a53a0dadeb9 100644
--- a/libraries/SITL/SIM_DroneCANDevice.cpp
+++ b/libraries/SITL/SIM_DroneCANDevice.cpp
@@ -217,9 +217,9 @@ void DroneCANDevice::update_rangefinder() {
     msg.sensor_id = 0;
     msg.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
     const float dist = AP::sitl()->get_rangefinder(0);
-    if (is_positive(dist)) {
+    if (!isnan(dist)) {
         msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
-        msg.range = dist;
+        msg.range = MAX(0, dist);
     } else {
         msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
         msg.range = 0;
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index a0b41ae95da3a7..a9a22dde116f32 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -556,7 +556,7 @@ void FlightAxis::update(const struct sitl_input &input)
     if (is_positive(dcm.c.z)) {
         rangefinder_m[0] = state.m_altitudeAGL_MTR / dcm.c.z;
     } else {
-        rangefinder_m[0] = -1;
+        rangefinder_m[0] = nanf("");
     }
 
     report_FPS();
diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index 81fb426190ed7c..e16ceabb8f9f80 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -553,13 +553,14 @@ void Frame::calculate_forces(const Aircraft &aircraft,
 
     Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
 
+    const auto *_sitl = AP::sitl();
     for (uint8_t i=0; i<num_motors; i++) {
         Vector3f mtorque, mthrust;
         motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage(), use_drag);
         torque += mtorque;
         thrust += mthrust;
         // simulate motor rpm
-        if (!is_zero(AP::sitl()->vibe_motor)) {
+        if (!is_zero(_sitl->vibe_motor)) {
             rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
         }
     }
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 72e8015b2bf096..f61b5168078f44 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -1021,7 +1021,7 @@ float SIM::get_rangefinder(uint8_t instance) {
     if (instance < ARRAY_SIZE(state.rangefinder_m)) {
         return state.rangefinder_m[instance];
     }
-    return -1;
+    return nanf("");
 };
 
 float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const