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; iget_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