From b7fb4022b479849082cdf2fd6155044613238c5f Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 24 Apr 2015 12:47:07 +0900 Subject: [PATCH] AVR_SITL: fix compile warnings re float constants --- libraries/AP_HAL_AVR_SITL/AnalogIn.cpp | 2 +- libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 14 ++++----- libraries/AP_HAL_AVR_SITL/sitl_gps.cpp | 38 ++++++++++++------------ libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 10 +++---- 4 files changed, 32 insertions(+), 32 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp index 7779154155..8dd6b6653e 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp @@ -47,7 +47,7 @@ float ADCSource::read_latest() { case ANALOG_INPUT_NONE: default: - return 0.0; + return 0.0f; } } diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index c7b18623ca..34ef2aed55 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -536,7 +536,7 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode) control.pwm[2] = ((control.pwm[2]-1000) * _sitl->engine_mul) + 1000; if (control.pwm[2] > 2000) control.pwm[2] = 2000; } - _motors_on = ((control.pwm[2]-1000)/1000.0) > 0; + _motors_on = ((control.pwm[2]-1000)/1000.0f) > 0; } else if (_vehicle == APMrover2) { // add in engine multiplier if (control.pwm[2] != 1500) { @@ -544,7 +544,7 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode) if (control.pwm[2] > 2000) control.pwm[2] = 2000; if (control.pwm[2] < 1000) control.pwm[2] = 1000; } - _motors_on = ((control.pwm[2]-1500)/500.0) != 0; + _motors_on = ((control.pwm[2]-1500)/500.0f) != 0; } else { _motors_on = false; // apply engine multiplier to first motor @@ -555,7 +555,7 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode) if (control.pwm[i] > 2000) control.pwm[i] = 2000; if (control.pwm[i] < 1000) control.pwm[i] = 1000; // update motor_on flag - if ((control.pwm[i]-1000)/1000.0 > 0) { + if ((control.pwm[i]-1000)/1000.0f > 0) { _motors_on = true; } } @@ -565,10 +565,10 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode) // lose 0.7V at full throttle float voltage = _sitl->batt_voltage - 0.7f*throttle; // assume 50A at full throttle - _current = 50.0 * throttle; + _current = 50.0f * throttle; // assume 3DR power brick - voltage_pin_value = ((voltage / 10.1) / 5.0) * 1024; - current_pin_value = ((_current / 17.0) / 5.0) * 1024; + voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024; + current_pin_value = ((_current / 17.0f) / 5.0f) * 1024; // setup wind control float wind_speed = _sitl->wind_speed * 100; @@ -607,7 +607,7 @@ Vector3f SITL_State::_rand_vec3f(void) Vector3f v = Vector3f(_rand_float(), _rand_float(), _rand_float()); - if (v.length() != 0.0) { + if (v.length() != 0.0f) { v.normalize(); } return v; diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp index 586657910f..6af5f749fc 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -101,7 +101,7 @@ int SITL_State::gps2_pipe(void) void SITL_State::_gps_write(const uint8_t *p, uint16_t size) { while (size--) { - if (_sitl->gps_byteloss > 0.0) { + if (_sitl->gps_byteloss > 0.0f) { float r = ((((unsigned)random()) % 1000000)) / 1.0e4; if (r < _sitl->gps_byteloss) { // lose the byte @@ -222,8 +222,8 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) pos.time = time_week_ms; pos.longitude = d->longitude * 1.0e7; pos.latitude = d->latitude * 1.0e7; - pos.altitude_ellipsoid = d->altitude*1000.0; - pos.altitude_msl = d->altitude*1000.0; + pos.altitude_ellipsoid = d->altitude*1000.0f; + pos.altitude_msl = d->altitude*1000.0f; pos.horizontal_accuracy = 1500; pos.vertical_accuracy = 2000; @@ -236,14 +236,14 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) status.uptime = hal.scheduler->millis(); velned.time = time_week_ms; - velned.ned_north = 100.0 * d->speedN; - velned.ned_east = 100.0 * d->speedE; - velned.ned_down = 100.0 * d->speedD; + velned.ned_north = 100.0f * d->speedN; + velned.ned_east = 100.0f * d->speedE; + velned.ned_down = 100.0f * d->speedD; velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100; velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100; - velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0; - if (velned.heading_2d < 0.0) { - velned.heading_2d += 360.0 * 100000.0; + velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; + if (velned.heading_2d < 0.0f) { + velned.heading_2d += 360.0f * 100000.0f; } velned.speed_accuracy = 40; velned.heading_accuracy = 4; @@ -312,9 +312,9 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d) p.longitude = d->longitude * 1.0e6; p.altitude = d->altitude * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; - p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0; - if (p.ground_course < 0.0) { - p.ground_course += 360.0 * 1000000.0; + p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f; + if (p.ground_course < 0.0f) { + p.ground_course += 360.0f * 1000000.0f; } p.satellites = d->have_lock?_sitl->gps_numsats:3; p.fix_type = d->have_lock?3:1; @@ -369,9 +369,9 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d) p.longitude = d->longitude * 1.0e6; p.altitude = d->altitude * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; - p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0; - if (p.ground_course < 0.0) { - p.ground_course += 360.0 * 100.0; + p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; + if (p.ground_course < 0.0f) { + p.ground_course += 360.0f * 100.0f; } p.satellites = d->have_lock?_sitl->gps_numsats:3; p.fix_type = d->have_lock?3:1; @@ -427,9 +427,9 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d) p.longitude = d->longitude * 1.0e7; p.altitude = d->altitude * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; - p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0; - if (p.ground_course < 0.0) { - p.ground_course += 360.0 * 100.0; + p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; + if (p.ground_course < 0.0f) { + p.ground_course += 360.0f * 100.0f; } p.satellites = d->have_lock?_sitl->gps_numsats:3; p.fix_type = d->have_lock?3:1; @@ -723,7 +723,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, if (_sitl->gps_drift_alt > 0) { // slow altitude drift - d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001*0.02); + d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001f*0.02f); } d.speedN = speedN; diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index a7bfee104f..ede7fb3af7 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -34,7 +34,7 @@ using namespace AVR_SITL; */ uint16_t SITL_State::_airspeed_sensor(float airspeed) { - const float airspeed_ratio = 1.9936; + const float airspeed_ratio = 1.9936f; const float airspeed_offset = 2013; float airspeed_pressure, airspeed_raw; @@ -80,8 +80,8 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed) float SITL_State::_gyro_drift(void) { - if (_sitl->drift_speed == 0.0f || - _sitl->drift_time == 0.0f) { + if (_sitl->drift_speed == 0.0f || + _sitl->drift_time == 0.0f) { return 0; } double period = _sitl->drift_time * 2; @@ -165,9 +165,9 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative // minimum noise levels are 2 bits, but averaged over many // samples, giving around 0.01 m/s/s - float accel_noise = 0.01; + float accel_noise = 0.01f; // minimum gyro noise is also less than 1 bit - float gyro_noise = ToRad(0.04); + float gyro_noise = ToRad(0.04f); if (_motors_on) { // add extra noise when the motors are on accel_noise += _sitl->accel_noise;