SITL: use NaN for invalid rangefinder data

needed to cope properly with terrain errors leading to negative rangefinder data
tridge · Aug 21, 2023 · e2fbdb8 · e2fbdb8
1 parent 5b9837f
commit e2fbdb8
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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