Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: eliminate float-equals issues

Browse Source
This commit is contained in:
Peter Barker 2018-08-15 12:37:10 +10:00 committed by Peter Barker
parent 8fee27937a
commit 3e9294a2ae
4 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_Aircraft.cpp
View File

@ -259,7 +259,7 @@ void Aircraft::setup_frame_time(float new_rate, float new_speedup)
/* adjust frame_time calculation */ /* adjust frame_time calculation */
void Aircraft::adjust_frame_time(float new_rate) void Aircraft::adjust_frame_time(float new_rate)
{ {
if (rate_hz != new_rate) { if (!is_equal(rate_hz, new_rate)) {
rate_hz = new_rate; rate_hz = new_rate;
frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz); frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
scaled_frame_time_us = frame_time_us/target_speedup; scaled_frame_time_us = frame_time_us/target_speedup;
@ -421,7 +421,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
} }
} }
if (last_speedup != sitl->speedup && sitl->speedup > 0) { if (!is_equal(last_speedup, float(sitl->speedup)) && sitl->speedup > 0) {
set_speedup(sitl->speedup); set_speedup(sitl->speedup);
last_speedup = sitl->speedup; last_speedup = sitl->speedup;
} }

6
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -428,7 +428,7 @@ void FlightAxis::update(const struct sitl_input &input)
state.m_accelerationBodyAZ_MPS2); state.m_accelerationBodyAZ_MPS2);
// accel on the ground is nasty in realflight, and prevents helicopter disarm // accel on the ground is nasty in realflight, and prevents helicopter disarm
if (state.m_isTouchingGround) { if (!is_zero(state.m_isTouchingGround)) {
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds; Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
accel_ef.z -= GRAVITY_MSS; accel_ef.z -= GRAVITY_MSS;
accel_body = dcm.transposed() * accel_ef; accel_body = dcm.transposed() * accel_ef;
@ -441,7 +441,7 @@ void FlightAxis::update(const struct sitl_input &input)
accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit); accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
// offset based on first position to account for offset in RF world // offset based on first position to account for offset in RF world
if (position_offset.is_zero() || state.m_resetButtonHasBeenPressed) { if (position_offset.is_zero() || !is_zero(state.m_resetButtonHasBeenPressed)) {
position_offset = position; position_offset = position;
} }
position -= position_offset; position -= position_offset;
@ -492,7 +492,7 @@ void FlightAxis::update(const struct sitl_input &input)
void FlightAxis::report_FPS(void) void FlightAxis::report_FPS(void)
{ {
if (frame_counter++ % 1000 == 0) { if (frame_counter++ % 1000 == 0) {
if (last_frame_count_s != 0) { if (!is_zero(last_frame_count_s)) {
uint64_t frames = socket_frame_counter - last_socket_frame_counter; uint64_t frames = socket_frame_counter - last_socket_frame_counter;
last_socket_frame_counter = socket_frame_counter; last_socket_frame_counter = socket_frame_counter;
double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s; double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;

2
libraries/SITL/SIM_Gripper_Servo.cpp
View File

@ -120,7 +120,7 @@ bool Gripper_Servo::should_report()
return false; return false;
} }
if (reported_position != position) { if (!is_equal(reported_position, position)) {
return true; return true;
} }

5
libraries/SITL/SIM_XPlane.cpp
View File

@ -240,11 +240,14 @@ bool XPlane::receive_data(void)
* input from XPlane10 * input from XPlane10
*/ */
bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale); bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (data[1] < 0 || if (data[1] < 0 ||
data[1] == throttle_sent || data[1] == throttle_sent ||
has_magic) { has_magic) {
break; break;
} }
#pragma GCC diagnostic pop
rcin[2] = data[1]; rcin[2] = data[1];
} }
break; break;
@ -387,7 +390,7 @@ void XPlane::send_data(const struct sitl_input &input)
if (SRV_Channels::find_channel(SRV_Channel::k_flap, flap_chan) || if (SRV_Channels::find_channel(SRV_Channel::k_flap, flap_chan) ||
SRV_Channels::find_channel(SRV_Channel::k_flap_auto, flap_chan)) { SRV_Channels::find_channel(SRV_Channel::k_flap_auto, flap_chan)) {
float flap = (input.servos[flap_chan]-1000)/1000.0; float flap = (input.servos[flap_chan]-1000)/1000.0;
if (flap != last_flap) { if (!is_equal(flap, last_flap)) {
send_dref("sim/flightmodel/controls/flaprqst", flap); send_dref("sim/flightmodel/controls/flaprqst", flap);
send_dref("sim/aircraft/overflow/acf_flap_arm", flap>0?1:0); send_dref("sim/aircraft/overflow/acf_flap_arm", flap>0?1:0);
} }