From 5631f865b2bcf4cac63b0e141d654ee6d5e16bfe Mon Sep 17 00:00:00 2001 From: James Bielman Date: Thu, 10 Jan 2013 10:42:24 -0800 Subject: [PATCH] Update floating point calculations to use floats instead of doubles. - Allows use of hardware floating point on the Cortex-M4. - Added "f" suffix to floating point literals. - Call floating point versions of stdlib math functions. --- APMrover2/Attitude.pde | 2 +- APMrover2/GCS_Mavlink.pde | 42 +++--- APMrover2/navigation.pde | 2 +- ArduCopter/ArduCopter.pde | 8 +- ArduCopter/Attitude.pde | 10 +- ArduCopter/GCS_Mavlink.pde | 60 ++++---- ArduCopter/Log.pde | 9 +- ArduCopter/commands.pde | 6 +- ArduCopter/commands_logic.pde | 4 +- ArduCopter/config.h | 130 +++++++++--------- ArduCopter/control_modes.pde | 8 +- ArduCopter/defines.h | 8 +- ArduCopter/navigation.pde | 24 ++-- ArduCopter/sensors.pde | 12 +- ArduCopter/test.pde | 6 +- ArduCopter/toy.pde | 2 +- ArduPlane/Attitude.pde | 12 +- ArduPlane/GCS_Mavlink.pde | 32 ++--- ArduPlane/navigation.pde | 10 +- Tools/ArduTracker/ArduTracker.pde | 4 +- Tools/ArduTracker/Attitude.pde | 38 ++--- Tools/ArduTracker/GCS_DebugTerminal.pde | 12 +- Tools/ArduTracker/GCS_Mavlink.pde | 14 +- Tools/ArduTracker/Mavlink_Common.h | 22 +-- Tools/ArduTracker/commands.pde | 6 +- Tools/ArduTracker/navigation.pde | 10 +- Tools/ArduTracker/sensors.pde | 2 +- Tools/CPUInfo/CPUInfo.pde | 14 +- libraries/APM_Control/AP_PitchController.cpp | 10 +- libraries/APM_Control/AP_RollController.cpp | 6 +- libraries/APM_Control/AP_YawController.cpp | 12 +- .../examples/AP_ADC_test/AP_ADC_test.pde | 2 +- libraries/AP_AHRS/AP_AHRS.cpp | 20 +-- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 70 +++++----- libraries/AP_AHRS/AP_AHRS_MPU6000.cpp | 22 +-- libraries/AP_Airspeed/AP_Airspeed.cpp | 6 +- libraries/AP_Baro/AP_Baro.cpp | 10 +- libraries/AP_Baro/AP_Baro_MS5611.cpp | 8 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 12 +- libraries/AP_Compass/Compass.cpp | 44 +++--- libraries/AP_Declination/AP_Declination.cpp | 4 +- libraries/AP_GPS/AP_GPS_HIL.cpp | 12 +- libraries/AP_GPS/GPS.cpp | 14 +- libraries/AP_HAL/utility/Print.cpp | 6 +- libraries/AP_HAL_AVR_SITL/sitl_gps.cpp | 2 +- libraries/AP_InertialNav/AP_InertialNav.cpp | 12 +- libraries/AP_InertialNav/AP_InertialNav.h | 8 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 22 +-- .../AP_InertialSensor/AP_InertialSensor.h | 6 +- .../AP_InertialSensor_MPU6000.cpp | 4 +- .../AP_InertialSensor_Oilpan.cpp | 12 +- libraries/AP_Math/AP_Math.cpp | 14 +- libraries/AP_Math/AP_Math.h | 8 +- libraries/AP_Math/examples/eulers/eulers.pde | 2 +- .../AP_Math/examples/location/location.pde | 2 +- libraries/AP_Math/location.cpp | 26 ++-- libraries/AP_Math/matrix3.cpp | 16 +-- libraries/AP_Math/quaternion.cpp | 22 +-- libraries/AP_Math/vector2.h | 6 +- libraries/AP_Math/vector3.cpp | 2 +- libraries/AP_Math/vector3.h | 6 +- libraries/AP_Motors/AP_MotorsHeli.cpp | 42 +++--- libraries/AP_Motors/AP_MotorsMatrix.cpp | 4 +- libraries/AP_Motors/AP_MotorsTri.cpp | 2 +- libraries/AP_Motors/AP_Motors_Class.cpp | 4 +- libraries/AP_Mount/AP_Mount.cpp | 42 +++--- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 12 +- libraries/AP_Param/AP_Param.cpp | 2 +- .../AP_RangeFinder_MaxsonarXL.cpp | 4 +- libraries/Filter/LowPassFilter.h | 2 +- .../Filter/examples/Derivative/Derivative.pde | 8 +- .../examples/LowPassFilter/LowPassFilter.pde | 6 +- libraries/PID/PID.cpp | 9 +- libraries/RC_Channel/RC_Channel.cpp | 2 +- 74 files changed, 538 insertions(+), 538 deletions(-) diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde index 5ab8e6644a..8ef8f8d229 100644 --- a/APMrover2/Attitude.pde +++ b/APMrover2/Attitude.pde @@ -23,7 +23,7 @@ static void throttle_slew_limit(int16_t last_throttle) // if slew limit rate is set to zero then do not slew limit if (g.throttle_slewrate) { // limit throttle change by the given percentage per second - float temp = g.throttle_slewrate * G_Dt * 0.01 * fabs(g.channel_throttle.radio_max - g.channel_throttle.radio_min); + float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min); // allow a minimum change of 1 PWM per cycle if (temp < 1) { temp = 1; diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 1c90944c0f..da329576ae 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1361,9 +1361,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } @@ -1371,10 +1371,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = -packet.z*1.0e2; + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } @@ -1383,10 +1383,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } @@ -1394,9 +1394,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; + tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } @@ -1557,7 +1557,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (var_type == AP_PARAM_INT32) { if (packet.param_value < 0) rounding_addition = -rounding_addition; float v = packet.param_value+rounding_addition; - v = constrain(v, -2147483648.0, 2147483647.0); + v = constrain(v, -2147483648.0f, 2147483647.0f); ((AP_Int32 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT16) { if (packet.param_value < 0) rounding_addition = -rounding_addition; @@ -1634,12 +1634,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_hil_state_decode(msg, &packet); float vel = pythagorous2(packet.vx, packet.vy); - float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100); + float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100); // set gps hil sensor g_gps->setHIL(packet.time_usec/1000, - packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, - vel*1.0e-2, cog*1.0e-2, 0, 10); + packet.lat*1.0e-7f, packet.lon*1.0e-7f, packet.alt*1.0e-3f, + vel*1.0e-2f, cog*1.0e-2f, 0, 10); // rad/sec Vector3f gyros; @@ -1649,9 +1649,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // m/s/s Vector3f accels; - accels.x = packet.xacc * (GRAVITY_MSS/1000.0); - accels.y = packet.yacc * (GRAVITY_MSS/1000.0); - accels.z = packet.zacc * (GRAVITY_MSS/1000.0); + accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); + accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); + accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(gyros); diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 5d0ecc3209..5d8f123bb2 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -71,7 +71,7 @@ static void update_crosstrack(void) // Crosstrack Error // ---------------- if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line + crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); } diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 45520630d0..8dd06d5cd5 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -630,7 +630,7 @@ static int8_t CH7_wp_index; // Battery Sensors //////////////////////////////////////////////////////////////////////////////// // Battery Voltage of battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; +static float battery_voltage1 = LOW_VOLTAGE * 1.05f; // refers to the instant amp draw – based on an Attopilot Current sensor static float current_amps1; // refers to the total amps drawn – based on an Attopilot Current sensor @@ -1741,11 +1741,11 @@ void update_simple_mode(void) if (simple_counter == 1) { // roll - simple_cos_x = sin(radians(90 - delta)); + simple_cos_x = sinf(radians(90 - delta)); }else if (simple_counter > 2) { // pitch - simple_sin_y = cos(radians(90 - delta)); + simple_sin_y = cosf(radians(90 - delta)); simple_counter = 0; } @@ -2132,7 +2132,7 @@ static void update_altitude_est() } static void tuning(){ - tuning_value = (float)g.rc_6.control_in / 1000.0; + tuning_value = (float)g.rc_6.control_in / 1000.0f; g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1 switch(g.radio_tuning) { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 819a46c256..f82ec06831 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -708,7 +708,7 @@ static void update_throttle_cruise(int16_t throttle) } // calc average throttle if we are in a level hover if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { - throttle_avg = throttle_avg * .99 + (float)throttle * .01; + throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f; g.throttle_cruise = throttle_avg; } } @@ -802,7 +802,7 @@ get_throttle_accel(int16_t z_target_accel) z_accel_error = 0; } else { // calculate accel error and Filter with fc = 2 Hz - z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); + z_accel_error = z_accel_error + 0.11164f * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); } last_call_ms = now; @@ -937,7 +937,7 @@ get_throttle_rate(int16_t z_target_speed) z_rate_error = 0; } else { // calculate rate error and filter with cut off frequency of 2 Hz - z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error); + z_rate_error = z_rate_error + 0.20085f * ((z_target_speed - climb_rate) - z_rate_error); } last_call_ms = now; @@ -1039,7 +1039,7 @@ get_throttle_althold_with_slew(int16_t target_alt, int16_t min_climb_rate, int16 static void get_throttle_rate_stabilized(int16_t target_rate) { - controller_desired_alt += target_rate * 0.02; + controller_desired_alt += target_rate * 0.02f; // do not let target altitude get too far from current altitude controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); @@ -1098,7 +1098,7 @@ get_throttle_surface_tracking(int16_t target_rate) } last_call_ms = now; - target_sonar_alt += target_rate * 0.02; + target_sonar_alt += target_rate * 0.02f; distance_error = (target_sonar_alt-sonar_alt); sonar_induced_slew_rate = constrain(fabs(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index fc73a1ec95..eb537f9767 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -198,7 +198,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack uint8_t battery_remaining = -1; if (current_total1 != 0 && g.pack_capacity != 0) { - battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); + battery_remaining = (100.0f * (g.pack_capacity - current_total1) / g.pack_capacity); } if (current_total1 != 0) { battery_current = current_amps1 * 100; @@ -264,12 +264,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) { mavlink_msg_nav_controller_output_send( chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - wp_bearing / 1.0e2, - wp_bearing / 1.0e2, - wp_distance / 1.0e2, - altitude_error / 1.0e2, + nav_roll / 1.0e2f, + nav_pitch / 1.0e2f, + wp_bearing / 1.0e2f, + wp_bearing / 1.0e2f, + wp_distance / 1.0e2f, + altitude_error / 1.0e2f, 0, crosstrack_error); // was 0 } @@ -434,12 +434,12 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) { mavlink_msg_vfr_hud_send( chan, - (float)g_gps->ground_speed / 100.0, - (float)g_gps->ground_speed / 100.0, + (float)g_gps->ground_speed / 100.0f, + (float)g_gps->ground_speed / 100.0f, (ahrs.yaw_sensor / 100) % 360, g.rc_3.servo_out/10, - current_loc.alt / 100.0, - climb_rate / 100.0); + current_loc.alt / 100.0f, + climb_rate / 100.0f); } static void NOINLINE send_raw_imu1(mavlink_channel_t chan) @@ -449,12 +449,12 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) mavlink_msg_raw_imu_send( chan, micros(), - accel.x * 1000.0 / GRAVITY_MSS, - accel.y * 1000.0 / GRAVITY_MSS, - accel.z * 1000.0 / GRAVITY_MSS, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, + accel.x * 1000.0f / GRAVITY_MSS, + accel.y * 1000.0f / GRAVITY_MSS, + accel.z * 1000.0f / GRAVITY_MSS, + gyro.x * 1000.0f, + gyro.y * 1000.0f, + gyro.z * 1000.0f, compass.mag_x, compass.mag_y, compass.mag_z); @@ -465,8 +465,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan) mavlink_msg_scaled_pressure_send( chan, millis(), - (float)barometer.get_pressure()/100.0, - (float)(barometer.get_pressure() - barometer.get_ground_pressure())/100.0, + (float)barometer.get_pressure()/100.0f, + (float)(barometer.get_pressure() - barometer.get_ground_pressure())/100.0f, (int)(barometer.get_temperature()*10)); } @@ -1344,10 +1344,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (tell_command.id < MAV_CMD_NAV_LAST) { // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) + x = tell_command.lat/1.0e7f; // local (x), global (latitude) + y = tell_command.lng/1.0e7f; // local (y), global (longitude) // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM - z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + z = tell_command.alt/1.0e2f; // local (z), global/relative (altitude) } // Switch to map APM command fields into MAVLink command fields @@ -1594,7 +1594,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) * case MAV_FRAME_LOCAL: // local (relative to home position) * { * tell_command.lat = 1.0e7*ToDeg(packet.x/ - * (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + * (radius_of_earth*cosf(ToRad(home.lat/1.0e7)))) + home.lat; * tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; * tell_command.alt = packet.z*1.0e2; * tell_command.options = MASK_OPTIONS_RELATIVE_ALT; @@ -1613,9 +1613,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) */ // we only are supporting Abs position, relative Alt - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; + tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2f; tell_command.options = 1; // store altitude relative to home alt!! Always!! switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields @@ -1865,7 +1865,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_hil_state_decode(msg, &packet); float vel = pythagorous2(packet.vx, packet.vy); - float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); + float cog = wrap_360(ToDeg(atan2f(packet.vx, packet.vy)) * 100); // set gps hil sensor g_gps->setHIL(packet.time_usec/1000, @@ -2050,8 +2050,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) send_text_P(SEVERITY_LOW,PSTR("bad fence point")); } else { Vector2l point; - point.x = packet.lat*1.0e7; - point.y = packet.lng*1.0e7; + point.x = packet.lat*1.0e7f; + point.y = packet.lng*1.0e7f; geofence_limit.set_fence_point_with_index(point, packet.idx); } break; @@ -2067,7 +2067,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else { Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx); mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), - point.x*1.0e-7, point.y*1.0e-7); + point.x*1.0e-7f, point.y*1.0e-7f); } break; } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 511cfecb1e..741fe25f0c 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -312,8 +312,8 @@ static void Log_Write_Current() LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), throttle_in : g.rc_3.control_in, throttle_integrator : throttle_integrator, - battery_voltage : (int16_t) (battery_voltage1 * 100.0), - current_amps : (int16_t) (current_amps1 * 100.0), + battery_voltage : (int16_t) (battery_voltage1 * 100.0f), + current_amps : (int16_t) (current_amps1 * 100.0f), current_total : (int16_t) current_total1 }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -329,8 +329,8 @@ static void Log_Read_Current() cliSerial->printf_P(PSTR("CURR, %d, %lu, %4.4f, %4.4f, %d\n"), (int)pkt.throttle_in, (unsigned long)pkt.throttle_integrator, - (float)pkt.battery_voltage/100.f, - (float)pkt.current_amps/100.f, + (float)pkt.battery_voltage/100.0f, + (float)pkt.current_amps/100.0f, (int)pkt.current_total); } @@ -1159,7 +1159,6 @@ static void Log_Read_Camera() { struct log_Camera pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - // 1 cliSerial->printf_P(PSTR("CAMERA, %lu, "),(unsigned long)pkt.gps_time); // 1 time print_latlon(cliSerial, pkt.latitude); // 2 lat diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index b930a2c272..b30159f864 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -188,9 +188,9 @@ static void init_home() Log_Write_Cmd(0, &home); // update navigation scalers. used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); + float rads = (fabsf((float)next_WP.lat)/t7) * 0.0174532925f; + scaleLongDown = cosf(rads); + scaleLongUp = 1.0f/cosf(rads); // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 06705097ce..d4f132fa4f 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -824,7 +824,7 @@ static void do_repeat_servo() event_timer = 0; event_value = command_cond_queue.alt; event_repeat = command_cond_queue.lat * 2; - event_delay = command_cond_queue.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_delay = command_cond_queue.lng * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds) switch(command_cond_queue.p1) { case CH_5: @@ -848,7 +848,7 @@ static void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; - event_delay = command_cond_queue.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_delay = command_cond_queue.lat * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds) event_repeat = command_cond_queue.alt * 2; update_events(); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9ce9e5e996..ea1b42fe5a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -114,7 +114,7 @@ # define RC_FAST_SPEED 125 # define RTL_YAW YAW_LOOK_AT_HOME # define TILT_COMPENSATION 5 - # define RATE_INTEGRATOR_LEAK_RATE 0.02 + # define RATE_INTEGRATOR_LEAK_RATE 0.02f # define RATE_ROLL_D 0 # define RATE_PITCH_D 0 # define HELI_PITCH_FF 0 @@ -377,17 +377,17 @@ // Battery monitoring // #ifndef LOW_VOLTAGE - # define LOW_VOLTAGE 9.6 + # define LOW_VOLTAGE 9.6f #endif #ifndef VOLT_DIV_RATIO - # define VOLT_DIV_RATIO 3.56 + # define VOLT_DIV_RATIO 3.56f #endif #ifndef CURR_AMP_PER_VOLT - # define CURR_AMP_PER_VOLT 27.32 + # define CURR_AMP_PER_VOLT 27.32f #endif #ifndef CURR_AMPS_OFFSET - # define CURR_AMPS_OFFSET 0.0 + # define CURR_AMPS_OFFSET 0.0f #endif #ifndef HIGH_DISCHARGE # define HIGH_DISCHARGE 1760 @@ -404,7 +404,7 @@ // INPUT_VOLTAGE // #ifndef INPUT_VOLTAGE - # define INPUT_VOLTAGE 5.0 + # define INPUT_VOLTAGE 5.0f #endif @@ -437,22 +437,22 @@ #endif // optical flow based loiter PI values #ifndef OPTFLOW_ROLL_P - #define OPTFLOW_ROLL_P 2.5 + #define OPTFLOW_ROLL_P 2.5f #endif #ifndef OPTFLOW_ROLL_I - #define OPTFLOW_ROLL_I 0.5 + #define OPTFLOW_ROLL_I 0.5f #endif #ifndef OPTFLOW_ROLL_D - #define OPTFLOW_ROLL_D 0.12 + #define OPTFLOW_ROLL_D 0.12f #endif #ifndef OPTFLOW_PITCH_P - #define OPTFLOW_PITCH_P 2.5 + #define OPTFLOW_PITCH_P 2.5f #endif #ifndef OPTFLOW_PITCH_I - #define OPTFLOW_PITCH_I 0.5 + #define OPTFLOW_PITCH_I 0.5f #endif #ifndef OPTFLOW_PITCH_D - #define OPTFLOW_PITCH_D 0.12 + #define OPTFLOW_PITCH_D 0.12f #endif #ifndef OPTFLOW_IMAX #define OPTFLOW_IMAX 1 @@ -543,7 +543,7 @@ // Y6 Support #ifndef TOP_BOTTOM_RATIO - # define TOP_BOTTOM_RATIO 1.00 + # define TOP_BOTTOM_RATIO 1.00f #endif @@ -738,26 +738,26 @@ // Extra motor values that are changed from time to time by jani @ jDrones as software // and charachteristics changes. #ifdef MOTORS_JD880 - # define STABILIZE_ROLL_P 3.7 - # define STABILIZE_ROLL_I 0.0 - # define STABILIZE_ROLL_IMAX 8.0 // degrees - # define STABILIZE_PITCH_P 3.7 - # define STABILIZE_PITCH_I 0.0 - # define STABILIZE_PITCH_IMAX 8.0 // degrees + # define STABILIZE_ROLL_P 3.7f + # define STABILIZE_ROLL_I 0.0f + # define STABILIZE_ROLL_IMAX 8.0f // degrees + # define STABILIZE_PITCH_P 3.7f + # define STABILIZE_PITCH_I 0.0f + # define STABILIZE_PITCH_IMAX 8.0f // degrees #endif #ifdef MOTORS_JD850 - # define STABILIZE_ROLL_P 4.2 - # define STABILIZE_ROLL_I 0.0 - # define STABILIZE_ROLL_IMAX 8.0 // degrees - # define STABILIZE_PITCH_P 4.2 - # define STABILIZE_PITCH_I 0.0 - # define STABILIZE_PITCH_IMAX 8.0 // degrees + # define STABILIZE_ROLL_P 4.2f + # define STABILIZE_ROLL_I 0.0f + # define STABILIZE_ROLL_IMAX 8.0f // degrees + # define STABILIZE_PITCH_P 4.2f + # define STABILIZE_PITCH_I 0.0f + # define STABILIZE_PITCH_IMAX 8.0f // degrees #endif #ifndef ACRO_P - # define ACRO_P 4.5 + # define ACRO_P 4.5f #endif #ifndef AXIS_LOCK_ENABLED @@ -766,33 +766,33 @@ // Good for smaller payload motors. #ifndef STABILIZE_ROLL_P - # define STABILIZE_ROLL_P 4.5 + # define STABILIZE_ROLL_P 4.5f #endif #ifndef STABILIZE_ROLL_I - # define STABILIZE_ROLL_I 0.0 + # define STABILIZE_ROLL_I 0.0f #endif #ifndef STABILIZE_ROLL_IMAX - # define STABILIZE_ROLL_IMAX 8.0 // degrees + # define STABILIZE_ROLL_IMAX 8.0f // degrees #endif #ifndef STABILIZE_PITCH_P - # define STABILIZE_PITCH_P 4.5 + # define STABILIZE_PITCH_P 4.5f #endif #ifndef STABILIZE_PITCH_I - # define STABILIZE_PITCH_I 0.0 + # define STABILIZE_PITCH_I 0.0f #endif #ifndef STABILIZE_PITCH_IMAX - # define STABILIZE_PITCH_IMAX 8.0 // degrees + # define STABILIZE_PITCH_IMAX 8.0f // degrees #endif #ifndef STABILIZE_YAW_P - # define STABILIZE_YAW_P 4.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy + # define STABILIZE_YAW_P 4.5f // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif #ifndef STABILIZE_YAW_I - # define STABILIZE_YAW_I 0.0 + # define STABILIZE_YAW_I 0.0f #endif #ifndef STABILIZE_YAW_IMAX - # define STABILIZE_YAW_IMAX 8.0 // degrees * 100 + # define STABILIZE_YAW_IMAX 8.0f // degrees * 100 #endif #ifndef YAW_LOOK_AHEAD_MIN_SPEED @@ -811,42 +811,42 @@ # define MAX_INPUT_PITCH_ANGLE 4500 #endif #ifndef RATE_ROLL_P - # define RATE_ROLL_P 0.150 + # define RATE_ROLL_P 0.150f #endif #ifndef RATE_ROLL_I - # define RATE_ROLL_I 0.100 + # define RATE_ROLL_I 0.100f #endif #ifndef RATE_ROLL_D - # define RATE_ROLL_D 0.004 + # define RATE_ROLL_D 0.004f #endif #ifndef RATE_ROLL_IMAX - # define RATE_ROLL_IMAX 5.0 // degrees + # define RATE_ROLL_IMAX 5.0f // degrees #endif #ifndef RATE_PITCH_P - # define RATE_PITCH_P 0.150 + # define RATE_PITCH_P 0.150f #endif #ifndef RATE_PITCH_I - # define RATE_PITCH_I 0.100 + # define RATE_PITCH_I 0.100f #endif #ifndef RATE_PITCH_D - # define RATE_PITCH_D 0.004 + # define RATE_PITCH_D 0.004f #endif #ifndef RATE_PITCH_IMAX - # define RATE_PITCH_IMAX 5.0 // degrees + # define RATE_PITCH_IMAX 5.0f // degrees #endif #ifndef RATE_YAW_P - # define RATE_YAW_P 0.25 + # define RATE_YAW_P 0.25f #endif #ifndef RATE_YAW_I - # define RATE_YAW_I 0.015 + # define RATE_YAW_I 0.015f #endif #ifndef RATE_YAW_D - # define RATE_YAW_D 0.000 + # define RATE_YAW_D 0.000f #endif #ifndef RATE_YAW_IMAX - # define RATE_YAW_IMAX 8.0 // degrees + # define RATE_YAW_IMAX 8.0f // degrees #endif @@ -882,10 +882,10 @@ // Loiter control gains // #ifndef LOITER_P - # define LOITER_P .20 + # define LOITER_P .20f #endif #ifndef LOITER_I - # define LOITER_I 0.0 + # define LOITER_I 0.0f #endif #ifndef LOITER_IMAX # define LOITER_IMAX 30 // degrees @@ -896,7 +896,7 @@ # define LOITER_REPOSITIONING DISABLED #endif #ifndef LOITER_REPOSITION_RATE - # define LOITER_REPOSITION_RATE 500.0 // cm/s + # define LOITER_REPOSITION_RATE 500.0f // cm/s #endif @@ -904,13 +904,13 @@ // Loiter Navigation control gains // #ifndef LOITER_RATE_P - # define LOITER_RATE_P 5.0 // + # define LOITER_RATE_P 5.0f // #endif #ifndef LOITER_RATE_I - # define LOITER_RATE_I 0.04 // Wind control + # define LOITER_RATE_I 0.04f // Wind control #endif #ifndef LOITER_RATE_D - # define LOITER_RATE_D 0.40 // try 2 or 3 for LOITER_RATE 1 + # define LOITER_RATE_D 0.40f // try 2 or 3 for LOITER_RATE 1 #endif #ifndef LOITER_RATE_IMAX # define LOITER_RATE_IMAX 30 // degrees @@ -920,13 +920,13 @@ // WP Navigation control gains // #ifndef NAV_P - # define NAV_P 2.4 // + # define NAV_P 2.4f // #endif #ifndef NAV_I - # define NAV_I 0.17 // Wind control + # define NAV_I 0.17f // Wind control #endif #ifndef NAV_D - # define NAV_D 0.00 // .95 + # define NAV_D 0.00f // .95 #endif #ifndef NAV_IMAX # define NAV_IMAX 18 // degrees @@ -963,10 +963,10 @@ #endif #ifndef ALT_HOLD_P - # define ALT_HOLD_P 2.0 + # define ALT_HOLD_P 2.0f #endif #ifndef ALT_HOLD_I - # define ALT_HOLD_I 0.0 + # define ALT_HOLD_I 0.0f #endif #ifndef ALT_HOLD_IMAX # define ALT_HOLD_IMAX 300 @@ -974,13 +974,13 @@ // RATE control #ifndef THROTTLE_P - # define THROTTLE_P 6.0 + # define THROTTLE_P 6.0f #endif #ifndef THROTTLE_I - # define THROTTLE_I 0.0 + # define THROTTLE_I 0.0f #endif #ifndef THROTTLE_D - # define THROTTLE_D 0.2 + # define THROTTLE_D 0.2f #endif #ifndef THROTTLE_IMAX @@ -1004,13 +1004,13 @@ // Throttle Accel control #ifndef THROTTLE_ACCEL_P - # define THROTTLE_ACCEL_P 0.75 + # define THROTTLE_ACCEL_P 0.75f #endif #ifndef THROTTLE_ACCEL_I - # define THROTTLE_ACCEL_I 1.50 + # define THROTTLE_ACCEL_I 1.50f #endif #ifndef THROTTLE_ACCEL_D - # define THROTTLE_ACCEL_D 0.0 + # define THROTTLE_ACCEL_D 0.0f #endif #ifndef THROTTLE_ACCEL_IMAX # define THROTTLE_ACCEL_IMAX 500 @@ -1021,7 +1021,7 @@ // Crosstrack compensation // #ifndef CROSSTRACK_GAIN - # define CROSSTRACK_GAIN .2 + # define CROSSTRACK_GAIN .2f #endif #ifndef CROSSTRACK_MIN_DISTANCE # define CROSSTRACK_MIN_DISTANCE 15 diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index d7cf4b429f..e552680b95 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -176,8 +176,8 @@ static void read_trim_switch() static void save_trim() { // save roll and pitch trim - float roll_trim = ToRad((float)g.rc_1.control_in/100.0); - float pitch_trim = ToRad((float)g.rc_2.control_in/100.0); + float roll_trim = ToRad((float)g.rc_1.control_in/100.0f); + float pitch_trim = ToRad((float)g.rc_2.control_in/100.0f); ahrs.add_trim(roll_trim, pitch_trim); } @@ -192,10 +192,10 @@ static void auto_trim() led_mode = SAVE_TRIM_LEDS; // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0); + float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0f); // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0); + float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0f); // make sure accelerometer values impact attitude quickly ahrs.set_fast_gains(true); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4aab1a7ceb..0d1b2b3aa7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -346,8 +346,8 @@ enum gcs_severity { #define DATA_RTL_REACHED_ALT 31 // battery monitoring macros -#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio -#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt +#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0f))*g.volt_div_ratio +#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0f))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt /* ************************************************************** */ /* Expansion PIN's that people can use for various things. */ @@ -394,8 +394,8 @@ enum gcs_severity { #define PIEZO_PIN AN5 //Last pin on the back ADC connector // RADIANS -#define RADX100 0.000174532925 -#define DEGX100 5729.57795 +#define RADX100 0.000174532925f +#define DEGX100 5729.57795f // EEPROM addresses diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 9b00c92aa8..aa5ee09eda 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -13,11 +13,11 @@ static void update_navigation() // used to calculate speed in X and Y, iterms // ------------------------------------------ - dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0; + dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0f; nav_last_gps_update = millis(); // prevent runup from bad GPS - dTnav = min(dTnav, 1.0); + dTnav = min(dTnav, 1.0f); // save GPS time nav_last_gps_time = g_gps->time; @@ -34,7 +34,7 @@ static void update_navigation() // check for inertial nav updates if( inertial_nav.position_ok() ) { // 50hz - dTnav = 0.02; // To-Do: calculate the time from the mainloop or INS readings? + dTnav = 0.02f; // To-Do: calculate the time from the mainloop or INS readings? // signal to run nav controllers pos_updated = true; @@ -108,7 +108,7 @@ static void calc_velocity_and_position(){ } // this speed is ~ in cm because we are using 10^7 numbers from GPS - float tmp = 1.0/dTnav; + float tmp = 1.0f/dTnav; #if INERTIAL_NAV_XY == ENABLED if( inertial_nav.position_ok() ) { @@ -338,11 +338,11 @@ static void update_nav_wp() //1 degree = 0.0174532925 radians // wrap - if (circle_angle > 6.28318531) - circle_angle -= 6.28318531; + if (circle_angle > 6.28318531f) + circle_angle -= 6.28318531f; - next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); - next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sin(1.57 - circle_angle)); + next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp); + next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle)); // use error as the desired rate towards the target // nav_lon, nav_lat is calculated @@ -563,8 +563,8 @@ static void calc_nav_rate(int16_t max_speed) // rotate by 90 to deal with trig functions temp = (9000l - wp_bearing) * RADX100; - temp_x = cos(temp); - temp_y = sin(temp); + temp_x = cosf(temp); + temp_y = sinf(temp); // rotate desired spped vector: int32_t x_target_speed = max_speed * temp_x - cross_speed * temp_y; @@ -633,7 +633,7 @@ static int16_t get_desired_speed(int16_t max_speed) int32_t s_min = WAYPOINT_SPEED_MIN; temp += s_min * s_min; if( temp < 0 ) temp = 0; // check to ensure we don't try to take the sqrt of a negative number - max_speed = sqrt((float)temp); + max_speed = sqrtf((float)temp); max_speed = min(max_speed, g.waypoint_speed_max); } } @@ -657,7 +657,7 @@ static void update_crosstrack(void) abs(wrap_180(wp_bearing - original_wp_bearing)) < 4500) { float temp = (wp_bearing - original_wp_bearing) * RADX100; - crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line + crosstrack_error = sinf(temp) * wp_distance; // Meters we are off track line }else{ // fade out crosstrack crosstrack_error >>= 1; diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 8addff15f5..613666ee6c 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -10,9 +10,9 @@ static void ReadSCP1000(void) { static void init_sonar(void) { #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC - sonar->calculate_scaler(g.sonar_type, 3.3); + sonar->calculate_scaler(g.sonar_type, 3.3f); #else - sonar->calculate_scaler(g.sonar_type, 5.0); + sonar->calculate_scaler(g.sonar_type, 5.0f); #endif } #endif @@ -28,7 +28,7 @@ static void init_barometer(void) static int32_t read_barometer(void) { barometer.read(); - return baro_filter.apply(barometer.get_altitude() * 100.0); + return baro_filter.apply(barometer.get_altitude() * 100.0f); } // return sonar altitude in centimeters @@ -37,7 +37,7 @@ static int16_t read_sonar(void) #if CONFIG_SONAR == ENABLED int16_t temp_alt = sonar->read(); - if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) { + if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70f) { if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) { sonar_alt_health++; } @@ -48,7 +48,7 @@ static int16_t read_sonar(void) #if SONAR_TILT_CORRECTION == 1 // correct alt for angle of the sonar float temp = cos_pitch_x * cos_roll_x; - temp = max(temp, 0.707); + temp = max(temp, 0.707f); temp_alt = (float)temp_alt * temp; #endif @@ -117,7 +117,7 @@ static void read_battery(void) if(g.battery_monitoring == 4) { batt_curr_analog_source->set_pin(g.battery_curr_pin); current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average()); - current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) + current_total1 += current_amps1 * 0.02778f; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) } // check for low voltage or current if the low voltage check hasn't already been triggered diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 68aed9500c..b61788ffa4 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -201,7 +201,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) for(int16_t i = 0; i < 200; i++){ int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100); - max_speed = sqrt((float)temp); + max_speed = sqrtf((float)temp); max_speed = min(max_speed, g.waypoint_speed_max); cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance); wp_distance += 100; @@ -561,8 +561,8 @@ test_gps(uint8_t argc, const Menu::arg *argv) * temp.c.x, temp.c.y, temp.c.z); * * int16_t _pitch = degrees(-asin(temp.c.x)); - * int16_t _roll = degrees(atan2(temp.c.y, temp.c.z)); - * int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x)); + * int16_t _roll = degrees(atan2f(temp.c.y, temp.c.z)); + * int16_t _yaw = degrees(atan2f(temp.b.x, temp.a.x)); * cliSerial->printf_P(PSTR( "angles\n" * "%d \t %d \t %d\n\n"), * _pitch, diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index e58b47c36b..91f2698928 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -116,7 +116,7 @@ void roll_pitch_toy() get_acro_yaw(0); yaw_timer--; - if((yaw_timer == 0) || (fabs(omega.z) < .17)) { + if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) { ap_system.yaw_stopped = true; nav_yaw = ahrs.yaw_sensor; } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3a0f176721..ae04fe999a 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -94,7 +94,7 @@ static void stabilize_pitch(float speed_scaler) { #if APM_CONTROL == DISABLED int32_t tempcalc = nav_pitch_cd + - fabs(ahrs.roll_sensor * g.kff_pitch_compensation) + + fabsf(ahrs.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (ahrs.pitch_sensor - g.pitch_trim_cd); if (inverted_flight) { @@ -123,12 +123,12 @@ static void stabilize_stick_mixing() float ch2_inf; ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; - ch1_inf = fabs(ch1_inf); + ch1_inf = fabsf(ch1_inf); ch1_inf = min(ch1_inf, 400.0); ch1_inf = ((400.0 - ch1_inf) /400.0); ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; - ch2_inf = fabs(ch2_inf); + ch2_inf = fabsf(ch2_inf); ch2_inf = min(ch2_inf, 400.0); ch2_inf = ((400.0 - ch2_inf) /400.0); @@ -156,7 +156,7 @@ static void stabilize_yaw(float speed_scaler) // important for steering on the ground during landing // ----------------------------------------------- ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; - ch4_inf = fabs(ch4_inf); + ch4_inf = fabsf(ch4_inf); ch4_inf = min(ch4_inf, 400.0); ch4_inf = ((400.0 - ch4_inf) /400.0); } @@ -328,7 +328,7 @@ static void calc_nav_roll() } // Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity. - nav_roll = ToDeg(atan(speed*turn_rate/GRAVITY_MSS)*100); + nav_roll = ToDeg(atanf(speed*turn_rate/GRAVITY_MSS)*100); #else // this is the old nav_roll calculation. We will use this for 2.50 @@ -362,7 +362,7 @@ static void throttle_slew_limit(int16_t last_throttle) // if slew limit rate is set to zero then do not slew limit if (g.throttle_slewrate) { // limit throttle change by the given percentage per second - float temp = g.throttle_slewrate * G_Dt * 0.01 * fabs(g.channel_throttle.radio_max - g.channel_throttle.radio_min); + float temp = g.throttle_slewrate * G_Dt * 0.01 * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min); // allow a minimum change of 1 PWM per cycle if (temp < 1) { temp = 1; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index de96752931..ee16f9fb0b 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -479,7 +479,7 @@ static void NOINLINE send_wind(mavlink_channel_t chan) Vector3f wind = ahrs.wind_estimate(); mavlink_msg_wind_send( chan, - degrees(atan2(-wind.y, -wind.x)), // use negative, to give + degrees(atan2f(-wind.y, -wind.x)), // use negative, to give // direction wind is coming from wind.length(), wind.z); @@ -1467,9 +1467,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } @@ -1477,10 +1477,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = -packet.z*1.0e2; + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } @@ -1489,10 +1489,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } @@ -1500,9 +1500,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; + tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } @@ -1800,7 +1800,7 @@ mission_failed: mavlink_msg_hil_state_decode(msg, &packet); float vel = pythagorous2(packet.vx, packet.vy); - float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100); + float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100); if (g_gps != NULL) { // set gps hil sensor diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 56fa6035de..4604bcfd45 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -53,7 +53,7 @@ static void navigate() // Disabled for now void calc_distance_error() { - distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01)); + distance_estimate += (float)g_gps->ground_speed * .0002f * cosf(radians(bearing_error_cd * .01f)); distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); wp_distance = max(distance_estimate,10); } @@ -165,7 +165,7 @@ static void update_loiter() * if (wp_distance < g.loiter_radius){ * nav_bearing += 9000; * }else{ - * nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); + * nav_bearing -= 100 * M_PI / 180 * asinf(g.loiter_radius / wp_distance); * } * * update_crosstrack(); @@ -182,9 +182,9 @@ static void update_crosstrack(void) Vector2f wind2d = Vector2f(wind.x, wind.y); float speed; if (ahrs.airspeed_estimate(&speed)) { - Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed; + Vector2f nav_vector = Vector2f(cosf(radians(nav_bearing_cd*0.01)), sinf(radians(nav_bearing_cd*0.01))) * speed; Vector2f nav_adjusted = nav_vector - wind2d; - nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100; + nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100; } } @@ -194,7 +194,7 @@ static void update_crosstrack(void) if (wp_totalDistance >= g.crosstrack_min_distance && abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { // Meters we are off track line - crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; + crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing_cd = wrap_360_cd(nav_bearing_cd); } diff --git a/Tools/ArduTracker/ArduTracker.pde b/Tools/ArduTracker/ArduTracker.pde index 386764ab79..3ef01a1891 100644 --- a/Tools/ArduTracker/ArduTracker.pde +++ b/Tools/ArduTracker/ArduTracker.pde @@ -742,8 +742,8 @@ void update_GPS(void) // Calculate new climb rate add_altitude_data(millis()/100, gps.altitude/10); - COGX = cos(ToRad(gps.ground_course/100.0)); - COGY = sin(ToRad(gps.ground_course/100.0)); + COGX = cosf(ToRad(gps.ground_course/100.0)); + COGY = sinf(ToRad(gps.ground_course/100.0)); } } diff --git a/Tools/ArduTracker/Attitude.pde b/Tools/ArduTracker/Attitude.pde index 4c3b2808b2..9b85db028e 100644 --- a/Tools/ArduTracker/Attitude.pde +++ b/Tools/ArduTracker/Attitude.pde @@ -6,21 +6,21 @@ void stabilize() { - float ch1_inf = 1.0; - float ch2_inf = 1.0; - float ch4_inf = 1.0; + float ch1_inf = 1.0f; + float ch2_inf = 1.0f; + float ch4_inf = 1.0f; #if AIRSPEED_SENSOR == ENABLED float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed); - speed_scaler = constrain(speed_scaler, 0.11, 9.0); + speed_scaler = constrain(speed_scaler, 0.11f, 9.0f); #endif #if AIRSPEED_SENSOR == DISABLED float speed_scaler; if (servo_out[CH_THROTTLE] > 0) - speed_scaler = 0.5 + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0); // First order taylor expansion of square root + speed_scaler = 0.5f + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0f); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't goint to implement that... else - speed_scaler = 1.67; - speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info + speed_scaler = 1.67f; + speed_scaler = constrain(speed_scaler, 0.6f, 1.67f); // This case is constrained tighter as we don't have real speed info #endif @@ -36,7 +36,7 @@ void stabilize() // Calculate dersired servo output for the roll // --------------------------------------------- servo_out[CH_ROLL] = pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler); - servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler); + servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler); //Serial.print(" servo_out[CH_ROLL] "); //Serial.print(servo_out[CH_ROLL],DEC); @@ -45,12 +45,12 @@ void stabilize() if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL); - ch1_inf = fabs(ch1_inf); + ch1_inf = fabsf(ch1_inf); ch1_inf = min(ch1_inf, 400.0); ch1_inf = ((400.0 - ch1_inf) /400.0); ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH); - ch2_inf = fabs(ch2_inf); + ch2_inf = fabsf(ch2_inf); ch2_inf = min(ch2_inf, 400.0); ch2_inf = ((400.0 - ch2_inf) /400.0); @@ -73,7 +73,7 @@ void stabilize() // ----------------------------------------------- if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER); - ch4_inf = fabs(ch4_inf); + ch4_inf = fabsf(ch4_inf); ch4_inf = min(ch4_inf, 400.0); ch4_inf = ((400.0 - ch4_inf) /400.0); } @@ -249,19 +249,19 @@ void set_servos_4(void) float x1,x2,y1,y2,x,y,r,z; y1 = 110600*current_loc.lat/t7; - x1 = (PI/180)*6378137*(cos(atan(0.99664719*tan(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7); + x1 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7); y2 = 110600*trackVehicle_loc.lat/t7; - x2 = (PI/180)*6378137*(cos(atan(0.99664719*tan(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7); + x2 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7); x = abs(x2 - x1); y = abs(y2 - y1); - r = sqrt(x*x+y*y); - z = trackVehicle_loc.alt/100.0 - current_loc.alt; + r = sqrtf(x*x+y*y); + z = trackVehicle_loc.alt/100.0f - current_loc.alt; - phi = (atan(z/r)*180/PI); - theta = (atan(x/y)*180/PI); + phi = (atanf(z/r)*180/PI); + theta = (atanf(x/y)*180/PI); // Check to see which quadrant of the angle if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng) @@ -294,8 +294,8 @@ void set_servos_4(void) Serial.print("theta: "); Serial.println(theta); // Outputing to the servos - servo_out[CH_ROLL] = 10000*phi/90.0; - servo_out[CH_PITCH] = 10000*theta/360.0; + servo_out[CH_ROLL] = 10000*phi/90.0f; + servo_out[CH_PITCH] = 10000*theta/360.0f; servo_out[CH_RUDDER] = 0; servo_out[CH_THROTTLE] = 0; diff --git a/Tools/ArduTracker/GCS_DebugTerminal.pde b/Tools/ArduTracker/GCS_DebugTerminal.pde index 423dbd7285..f1b09fe63d 100644 --- a/Tools/ArduTracker/GCS_DebugTerminal.pde +++ b/Tools/ArduTracker/GCS_DebugTerminal.pde @@ -781,8 +781,8 @@ GCS_DEBUGTERMINAL::run_debugt_command(char *buf) port->printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"), (float)nav_pitch / 100, (float)dcm.pitch_sensor / 100, - fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100, - (float)(nav_pitch-dcm.pitch_sensor + fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100); + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100, + (float)(nav_pitch-dcm.pitch_sensor + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100); port->printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"), (float)servo_out[CH_PITCH] / 100); @@ -1275,12 +1275,12 @@ void GCS_DEBUGTERMINAL::print_rlocation(Location *wp) { //float x, y; - //y = (float)(wp->lat - current_loc.lat) * 0.0111194927; - //x = (float)(wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; + //y = (float)(wp->lat - current_loc.lat) * 0.0111194927f; + //x = (float)(wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f; BetterStream *port = _port; int x, y; - y = (wp->lat - current_loc.lat) * 0.0111194927; - x = (wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; + y = (wp->lat - current_loc.lat) * 0.0111194927f; + x = (wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f; port->printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"), abs(y), (y >= 0 ? 'N' : 'S'), abs(x), (x >= 0 ? 'E' : 'W'), diff --git a/Tools/ArduTracker/GCS_Mavlink.pde b/Tools/ArduTracker/GCS_Mavlink.pde index dcaa6b753a..4eda246f17 100644 --- a/Tools/ArduTracker/GCS_Mavlink.pde +++ b/Tools/ArduTracker/GCS_Mavlink.pde @@ -490,18 +490,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { case MAV_FRAME_GLOBAL: { - tell_command.lng = 1.0e7*packet.x; - tell_command.lat = 1.0e7*packet.y; - tell_command.alt = packet.z*1.0e2; + tell_command.lng = 1.0e7f*packet.x; + tell_command.lat = 1.0e7f*packet.y; + tell_command.alt = packet.z*1.0e2f; break; } case MAV_FRAME_LOCAL: // local (relative to home position) { - tell_command.lng = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng; - tell_command.lat = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat; - tell_command.alt = -packet.z*1.0e2 + home.alt; + tell_command.lng = 1.0e7f*ToDeg(packet.x/ + (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lng; + tell_command.lat = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lat; + tell_command.alt = -packet.z*1.0e2f + home.alt; break; } } diff --git a/Tools/ArduTracker/Mavlink_Common.h b/Tools/ArduTracker/Mavlink_Common.h index 1372de6d04..48df48c6fd 100644 --- a/Tools/ArduTracker/Mavlink_Common.h +++ b/Tools/ArduTracker/Mavlink_Common.h @@ -120,26 +120,26 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui { float gamma = dcm.pitch; // neglecting angle of attack for now float yaw = dcm.yaw; - mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7, - current_loc.lng/1.0e7,current_loc.alt/1.0e2,gps.ground_speed/1.0e2*cos(gamma)*cos(yaw), - gps.ground_speed/1.0e2*cos(gamma)*sin(yaw),gps.ground_speed/1.0e2*sin(gamma)); + mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7f, + current_loc.lng/1.0e7f,current_loc.alt/1.0e2f,gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw), + gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma)); break; } case MSG_LOCAL_LOCATION: { float gamma = dcm.pitch; // neglecting angle of attack for now float yaw = dcm.yaw; - mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth, - ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)), - (current_loc.alt-home.alt)/1.0e2, gps.ground_speed/1.0e2*cos(gamma)*cos(yaw), - gps.ground_speed/1.0e2*cos(gamma)*sin(yaw),gps.ground_speed/1.0e2*sin(gamma)); + mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7f)*radius_of_earth, + ToRad((current_loc.lng-home.lng)/1.0e7f)*radius_of_earth*cosf(ToRad(home.lat/1.0e7f)), + (current_loc.alt-home.alt)/1.0e2f, gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw), + gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma)); break; } case MSG_GPS_RAW: { mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(), - gps.latitude/1.0e7,gps.longitude/1.0e7,gps.altitude/100.0, - 2.0,10.0,gps.ground_speed/100.0,gps.ground_course/100.0); + gps.latitude/1.0e7f,gps.longitude/1.0e7f,gps.altitude/100.0f, + 2.0f,10.0f,gps.ground_speed/100.0f,gps.ground_course/100.0f); break; } case MSG_AIRSPEED: @@ -179,8 +179,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); mavlink_msg_raw_imu_send(chan,timeStamp, - accel.x*1000.0/gravity,accel.y*1000.0/gravity,accel.z*1000.0/gravity, - gyro.x*1000.0,gyro.y*1000.0,gyro.z*1000.0, + accel.x*1000.0f/gravity,accel.y*1000.0f/gravity,accel.z*1000.0f/gravity, + gyro.x*1000.0f,gyro.y*1000.0f,gyro.z*1000.0f, compass.mag_x,compass.mag_y,compass.mag_z); mavlink_msg_raw_pressure_send(chan,timeStamp, adc.Ch(AIRSPEED_CH),pitot.RawPress,0); diff --git a/Tools/ArduTracker/commands.pde b/Tools/ArduTracker/commands.pde index ef520d6596..1633e9a735 100644 --- a/Tools/ArduTracker/commands.pde +++ b/Tools/ArduTracker/commands.pde @@ -178,10 +178,10 @@ void set_next_WP(struct Location *wp) loiter_sum = 0; loiter_total = 0; - float rads = (abs(next_WP.lat)/t7) * 0.0174532925; + float rads = (abs(next_WP.lat)/t7) * 0.0174532925f; //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); + scaleLongDown = cosf(rads); + scaleLongUp = 1.0f/cosf(rads); // this is handy for the groundstation wp_totalDistance = getDistance(¤t_loc, &next_WP); diff --git a/Tools/ArduTracker/navigation.pde b/Tools/ArduTracker/navigation.pde index 7e744e5f56..ba5fdda4db 100644 --- a/Tools/ArduTracker/navigation.pde +++ b/Tools/ArduTracker/navigation.pde @@ -91,7 +91,7 @@ void update_navigation() Disabled for now void calc_distance_error() { - //distance_estimate += (float)gps.ground_speed * .0002 * cos(radians(bearing_error * .01)); + //distance_estimate += (float)gps.ground_speed * .0002f * cosf(radians(bearing_error * .01f)); //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); //wp_distance = max(distance_estimate,10); } @@ -134,7 +134,7 @@ void calc_altitude_error() // special thanks to Ryan Beall for this one float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) pitch_angle = constrain(pitch_angle, -2000, 2000); - float scale = sin(radians(pitch_angle * .01)); + float scale = sinf(radians(pitch_angle * .01)); altitude_estimate += (float)airspeed * .0002 * scale; altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); @@ -195,7 +195,7 @@ void update_crosstrack(void) // Crosstrack Error // ---------------- if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line + crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * get(PARAM_XTRACK_GAIN), -get(PARAM_XTRACK_ANGLE), get(PARAM_XTRACK_ANGLE)); nav_bearing = wrap_360(nav_bearing); } @@ -223,7 +223,7 @@ long getDistance(struct Location *loc1, struct Location *loc2) return -1; float dlat = (float)(loc2->lat - loc1->lat); float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; + return sqrtf(sq(dlat) + sq(dlong)) * .01113195f; } long get_alt_distance(struct Location *loc1, struct Location *loc2) @@ -235,7 +235,7 @@ long get_bearing(struct Location *loc1, struct Location *loc2) { long off_x = loc2->lng - loc1->lng; long off_y = (loc2->lat - loc1->lat) * scaleLongUp; - long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + long bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f; if (bearing < 0) bearing += 36000; return bearing; } diff --git a/Tools/ArduTracker/sensors.pde b/Tools/ArduTracker/sensors.pde index 16f483ee4d..555e0f13f7 100644 --- a/Tools/ArduTracker/sensors.pde +++ b/Tools/ArduTracker/sensors.pde @@ -28,7 +28,7 @@ void read_airspeed(void) airpressure_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75); airpressure = (int)airpressure_raw - airpressure_offset; airpressure = max(airpressure, 0); - airspeed = sqrt((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100; + airspeed = sqrtf((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100; #endif calc_airspeed_errors(); diff --git a/Tools/CPUInfo/CPUInfo.pde b/Tools/CPUInfo/CPUInfo.pde index a8878ab6a6..addf8d7b34 100644 --- a/Tools/CPUInfo/CPUInfo.pde +++ b/Tools/CPUInfo/CPUInfo.pde @@ -100,13 +100,13 @@ static void show_timings(void) TIMEIT("dmul", v_out_d *= v_d, 100); TIMEIT("ddiv", v_out_d /= v_d, 100); - TIMEIT("sin()", v_out = sin(v_f), 20); - TIMEIT("cos()", v_out = cos(v_f), 20); - TIMEIT("tan()", v_out = tan(v_f), 20); - TIMEIT("acos()", v_out = acos(v_f * 0.2), 20); - TIMEIT("asin()", v_out = asin(v_f * 0.2), 20); - TIMEIT("atan2()", v_out = atan2(v_f * 0.2, v_f * 0.3), 20); - TIMEIT("sqrt()",v_out = sqrt(v_f), 20); + TIMEIT("sin()", v_out = sinf(v_f), 20); + TIMEIT("cos()", v_out = cosf(v_f), 20); + TIMEIT("tan()", v_out = tanf(v_f), 20); + TIMEIT("acos()", v_out = acosf(v_f * 0.2), 20); + TIMEIT("asin()", v_out = asinf(v_f * 0.2), 20); + TIMEIT("atan2()", v_out = atan2f(v_f * 0.2, v_f * 0.3), 20); + TIMEIT("sqrt()",v_out = sqrtf(v_f), 20); TIMEIT("iadd8", v_out_8 += v_8, 100); TIMEIT("isub8", v_out_8 -= v_8, 100); diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 6125cf2e97..966fddb1b3 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -37,7 +37,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab _last_t = tnow; if(_ahrs == NULL) return 0; - float delta_time = (float)dt / 1000.0; + float delta_time = (float)dt / 1000.0f; int8_t inv = 1; if(abs(_ahrs->roll_sensor)>9000) inv = -1; @@ -47,12 +47,12 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab float rate = _ahrs->get_pitch_rate_earth(); - float RC = 1/(2*M_PI*_fCut); + float RC = 1/(2*PI*_fCut); rate = _last_rate + (delta_time / (RC + delta_time)) * (rate - _last_rate); _last_rate = rate; - float roll_scaler = 1/constrain(cos(_ahrs->roll),.33,1); + float roll_scaler = 1/constrain(cosf(_ahrs->roll),0.33f,1); int32_t desired_rate = angle_err * _kp_angle; @@ -70,7 +70,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab int32_t rate_error = desired_rate - ToDeg(rate)*100; - float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0); + float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0f); if(roll_ff > 4500) roll_ff = 4500; else if(roll_ff < 0) @@ -80,7 +80,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab //rate integrator if (!stabilize) { - if ((fabs(_ki_rate) > 0) && (dt > 0)) + if ((fabsf(_ki_rate) > 0) && (dt > 0)) { _integrator += (rate_error * _ki_rate) * scaler * delta_time; if (_integrator < -4500-out) _integrator = -4500-out; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 2499c80716..64c81ea1fc 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -34,13 +34,13 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi _last_t = tnow; if(_ahrs == NULL) return 0; // can't control without a reference - float delta_time = (float)dt / 1000.0; + float delta_time = (float)dt / 1000.0f; int32_t angle_err = angle - _ahrs->roll_sensor; float rate = _ahrs->get_roll_rate_earth(); - float RC = 1/(2*M_PI*_fCut); + float RC = 1/(2*PI*_fCut); rate = _last_rate + (delta_time / (RC + delta_time)) * (rate - _last_rate); _last_rate = rate; @@ -60,7 +60,7 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi //rate integrator if (!stabilize) { - if ((fabs(_ki_rate) > 0) && (dt > 0)) + if ((fabsf(_ki_rate) > 0) && (dt > 0)) { _integrator += (rate_error * _ki_rate) * scaler * delta_time; if (_integrator < -4500-out) _integrator = -4500-out; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 27bbc82d47..6a79ca9370 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -22,8 +22,8 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { // Low pass filter cut frequency for derivative calculation. // FCUT macro computes a frequency cut based on an acceptable delay. -#define FCUT(d) (1 / ( 2 * 3.14 * (d) ) ) -const float AP_YawController::_fCut = FCUT(.5); +#define FCUT(d) (1 / ( 2 * 3.14f * (d) ) ) +const float AP_YawController::_fCut = FCUT(0.5f); int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) { @@ -38,7 +38,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) return 0; } - float delta_time = (float) dt / 1000.0; + float delta_time = (float) dt / 1000.0f; if(stick_movement) { if(!_stick_movement) { @@ -54,20 +54,20 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) Vector3f accels = _ins->get_accel(); // I didn't pull 512 out of a hat - it is a (very) loose approximation of - // 100*ToDeg(asin(-accels.y/9.81)) + // 100*ToDeg(asinf(-accels.y/9.81f)) // which, with a P of 1.0, would mean that your rudder angle would be // equal to your roll angle when // the plane is still. Thus we have an (approximate) unit to go by. float error = 512 * -accels.y; // strongly filter the error - float RC = 1/(2*M_PI*_fCut); + float RC = 1/(2*PI*_fCut); error = _last_error + (delta_time / (RC + delta_time)) * (error - _last_error); _last_error = error; // integrator if(_freeze_start_time < (tnow - 2000)) { - if ((fabs(_ki) > 0) && (dt > 0)) { + if ((fabsf(_ki) > 0) && (dt > 0)) { _integrator += (error * _ki) * scaler * delta_time; if (_integrator < -_imax) _integrator = -_imax; else if (_integrator > _imax) _integrator = _imax; diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index 18fd3af6da..bab902d3ce 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -82,7 +82,7 @@ static void show_data() for (i=0; i<6; i++) { if (result[i] < min[i]) min[i] = result[i]; if (result[i] > max[i]) max[i] = result[i]; - if (fabs(result[i]) > 0x8000) { + if (fabsf(result[i]) > 0x8000) { hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]); } } diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 6e96eba54a..a68278422e 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -19,7 +19,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // @Description: This controls how how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0. // @Range: 0.0 1.0 // @Increment: .01 - AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0), + AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f), // @Param: GPS_USE // @DisplayName: AHRS use GPS for navigation @@ -32,14 +32,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly. // @Range: 0.1 0.4 // @Increment: .01 - AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.4), + AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.4f), // @Param: RP_P // @DisplayName: AHRS RP_P // @Description: This controls how fast the accelerometers correct the attitude // @Range: 0.1 0.4 // @Increment: .01 - AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4), + AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4f), // @Param: WIND_MAX // @DisplayName: Maximum wind @@ -47,7 +47,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // @Range: 0 127 // @Units: m/s // @Increment: 1 - AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0), + AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f), // @Param: BARO_USE // @DisplayName: AHRS Use Barometer @@ -89,13 +89,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { float AP_AHRS::get_pitch_rate_earth(void) { Vector3f omega = get_gyro(); - return cos(roll) * omega.y - sin(roll) * omega.z; + return cosf(roll) * omega.y - sinf(roll) * omega.z; } // get roll rate in earth frame, in radians/s float AP_AHRS::get_roll_rate_earth(void) { Vector3f omega = get_gyro(); - return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll)); + return omega.x + tanf(pitch)*(omega.y*sinf(roll) + omega.z*cosf(roll)); } // return airspeed estimate if available @@ -107,8 +107,8 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) // constrain the airspeed by the ground speed // and AHRS_WIND_MAX *airspeed_ret = constrain(*airspeed_ret, - _gps->ground_speed*0.01 - _wind_max, - _gps->ground_speed*0.01 + _wind_max); + _gps->ground_speed*0.01f - _wind_max, + _gps->ground_speed*0.01f + _wind_max); } return true; } @@ -121,8 +121,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ Vector3f trim = _trim.get(); // add new trim - trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0)); - trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0)); + trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0f), ToRad(10.0f)); + trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0f), ToRad(10.0f)); // set new trim values _trim.set(trim); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index cac1bf5b06..48342d1ed1 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -48,7 +48,7 @@ AP_AHRS_DCM::update(void) // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter - if (delta_t > 0.2) { + if (delta_t > 0.2f) { _ra_sum.zero(); _ra_deltat = 0; return; @@ -129,14 +129,14 @@ AP_AHRS_DCM::check_matrix(void) // the pitch calculation via asin(). These NaN values can // feed back into the rest of the DCM matrix via the // error_course value. - if (!(_dcm_matrix.c.x < 1.0 && - _dcm_matrix.c.x > -1.0)) { + if (!(_dcm_matrix.c.x < 1.0f && + _dcm_matrix.c.x > -1.0f)) { // We have an invalid matrix. Force a normalisation. renorm_range_count++; normalize(); if (_dcm_matrix.is_nan() || - fabs(_dcm_matrix.c.x) > 10) { + fabsf(_dcm_matrix.c.x) > 10) { // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", @@ -172,16 +172,16 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result) // we don't want to compound the error by making DCM less // accurate. - renorm_val = 1.0 / a.length(); + renorm_val = 1.0f / a.length(); // keep the average for reporting _renorm_val_sum += renorm_val; _renorm_val_count++; - if (!(renorm_val < 2.0 && renorm_val > 0.5)) { + if (!(renorm_val < 2.0f && renorm_val > 0.5f)) { // this is larger than it should get - log it as a warning renorm_range_count++; - if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) { + if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) { // we are getting values which are way out of // range, we will reset the matrix and hope we // can recover our attitude using drift @@ -245,8 +245,8 @@ AP_AHRS_DCM::yaw_error_compass(void) } // get the earths magnetic field (only X and Y components needed) - Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), - sin(_compass->get_declination()), 0); + Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()), + sinf(_compass->get_declination()), 0); // calculate the error term in earth frame Vector3f error = rb % mag_earth; @@ -259,7 +259,7 @@ AP_AHRS_DCM::yaw_error_compass(void) float AP_AHRS_DCM::yaw_error_gps(void) { - return sin(ToRad(_gps->ground_course * 0.01) - yaw); + return sinf(ToRad(_gps->ground_course * 0.01f) - yaw); } @@ -270,10 +270,10 @@ float AP_AHRS_DCM::_P_gain(float spin_rate) { if (spin_rate < ToDeg(50)) { - return 1.0; + return 1.0f; } if (spin_rate > ToDeg(500)) { - return 10.0; + return 10.0f; } return spin_rate/ToDeg(50); } @@ -299,7 +299,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) if (_compass && _compass->use_for_yaw()) { if (_compass->last_update != _compass_last_update) { - yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; + yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f; _compass_last_update = _compass->last_update; // we force an additional compass read() // here. This has the effect of throwing away @@ -316,10 +316,10 @@ AP_AHRS_DCM::drift_correction_yaw(void) } else if (_fly_forward && have_gps()) { if (_gps->last_fix_time != _gps_last_update && _gps->ground_speed >= GPS_SPEED_MIN) { - yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3; + yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3f; _gps_last_update = _gps->last_fix_time; if (!_have_initial_yaw) { - _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01)); + _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01f)); _omega_yaw_P.zero(); _have_initial_yaw = true; } @@ -332,7 +332,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) // we don't have any new yaw information // slowly decay _omega_yaw_P to cope with loss // of our yaw source - _omega_yaw_P *= 0.97; + _omega_yaw_P *= 0.97f; return; } @@ -358,12 +358,12 @@ AP_AHRS_DCM::drift_correction_yaw(void) // don't update the drift term if we lost the yaw reference // for more than 2 seconds - if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) { + if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) { // also add to the I term _omega_I_sum.z += error.z * _ki_yaw * yaw_deltat; } - _error_yaw_sum += fabs(yaw_error); + _error_yaw_sum += fabsf(yaw_error); _error_yaw_count++; } @@ -411,7 +411,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction - if (_ra_deltat < 0.2) { + if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } @@ -489,8 +489,8 @@ AP_AHRS_DCM::drift_correction(float deltat) Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. - vdelta.z = constrain(vdelta.z, -0.5, 0.5); - GA_e = Vector3f(0, 0, -1.0) + vdelta; + vdelta.z = constrain(vdelta.z, -0.5f, 0.5f); + GA_e = Vector3f(0, 0, -1.0f) + vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information @@ -500,7 +500,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); float length = GA_b.length(); - if (length > 1.0) { + if (length > 1.0f) { GA_b /= length; if (GA_b.is_inf()) { // wait for some non-zero acceleration information @@ -518,10 +518,10 @@ AP_AHRS_DCM::drift_correction(float deltat) float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 - float theta = atan2(GA_b.y, GA_b.x); + float theta = atan2f(GA_b.y, GA_b.x); // equation 12 - Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z); + Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; @@ -533,8 +533,8 @@ AP_AHRS_DCM::drift_correction(float deltat) // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (_compass && _compass->use_for_yaw()) { - if (have_gps() && gps_gain == 1.0) { - error.z *= sin(fabs(roll)); + if (have_gps() && gps_gain == 1.0f) { + error.z *= sinf(fabsf(roll)); } else { error.z = 0; } @@ -617,7 +617,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) } float diff_length = fuselageDirectionDiff.length(); - if (diff_length > 0.2) { + if (diff_length > 0.2f) { // when turning, use the attitude response to estimate // wind speed float V; @@ -632,18 +632,18 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; Vector3f velocitySum = velocity + _last_vel; - float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x); - float sintheta = sin(theta); - float costheta = cos(theta); + float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x); + float sintheta = sinf(theta); + float costheta = cosf(theta); Vector3f wind = Vector3f(); wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y); wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y); wind.z = velocitySum.z - V * fuselageDirectionSum.z; - wind *= 0.5; + wind *= 0.5f; if (wind.length() < _wind.length() + 20) { - _wind = _wind * 0.95 + wind * 0.05; + _wind = _wind * 0.95f + wind * 0.05f; } _last_wind_time = now; @@ -651,7 +651,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) // when flying straight use airspeed to get wind estimate if available Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed(); Vector3f wind = velocity - airspeed; - _wind = _wind * 0.92 + wind * 0.08; + _wind = _wind * 0.92f + wind * 0.08f; } } @@ -734,8 +734,8 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) // constrain the airspeed by the ground speed // and AHRS_WIND_MAX *airspeed_ret = constrain(*airspeed_ret, - _gps->ground_speed*0.01 - _wind_max, - _gps->ground_speed*0.01 + _wind_max); + _gps->ground_speed*0.01f - _wind_max, + _gps->ground_speed*0.01f + _wind_max); } return ret; } diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index 08090b9a49..e8194b12ae 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -79,11 +79,11 @@ AP_AHRS_MPU6000::update(void) // TO-DO: should remove and replace with more standard functions float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians) { - if( angle_in_radians > M_PI ) { - return(angle_in_radians - 2*M_PI); + if( angle_in_radians > PI ) { + return(angle_in_radians - 2*PI); } - else if( angle_in_radians < -M_PI ) { - return(angle_in_radians + 2*M_PI); + else if( angle_in_radians < -PI ) { + return(angle_in_radians + 2*PI); } else{ return(angle_in_radians); @@ -141,13 +141,13 @@ void AP_AHRS_MPU6000::drift_correction( float deltat ) // TO-DO: fix this. Currently it makes the roll and pitch drift more! // If bias values are greater than 1 LSB we update the hardware offset // registers - if( fabs(_gyro_bias[0])>1.0 ) { + if( fabsf(_gyro_bias[0])>1.0f ) { //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0); //_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0); //_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that // we have already corrected on registers... } - if (fabs(_gyro_bias[1])>1.0) { + if (fabsf(_gyro_bias[1])>1.0f) { //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0); //_gyro_bias[1] -= (int)_gyro_bias[1]; } @@ -199,9 +199,9 @@ void AP_AHRS_MPU6000::push_gains_to_dmp() { uint8_t gain; - if( _kp.get() >= 1.0 ) { + if( _kp.get() >= 1.0f ) { gain = 0xFF; - }else if( _kp.get() <= 0.0 ) { + }else if( _kp.get() <= 0.0f ) { gain = 0x00; }else{ gain = (uint8_t)((float)0xFF * _kp.get()); @@ -226,8 +226,8 @@ AP_AHRS_MPU6000::yaw_error_compass(void) } // get the earths magnetic field (only X and Y components needed) - Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), - sin(_compass->get_declination()), 0); + Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()), + sinf(_compass->get_declination()), 0); // calculate the error term in earth frame Vector3f error = rb % mag_earth; @@ -290,7 +290,7 @@ AP_AHRS_MPU6000::drift_correction_yaw(void) yaw_error = wrap_PI(heading - yaw_corrected); // shift the corrected yaw towards the compass heading a bit - yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1); + yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1f); // rebuild the dcm matrix yet again _dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected); diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 02320cfefe..7bcdb0a864 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -40,7 +40,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { // @DisplayName: Airspeed ratio // @Description: Airspeed calibration ratio // @Increment: 0.1 - AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936), + AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936f), AP_GROUPEND }; @@ -73,6 +73,6 @@ void AP_Airspeed::read(void) } _airspeed_raw = _source->read_average(); airspeed_pressure = max(_airspeed_raw - _offset, 0); - _airspeed = 0.7 * _airspeed + - 0.3 * sqrt(airspeed_pressure * _ratio); + _airspeed = 0.7f * _airspeed + + 0.3f * sqrtf(airspeed_pressure * _ratio); } diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 101b314740..0bc4e230a6 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -83,9 +83,9 @@ void AP_Baro::calibrate() "for more than 500ms in AP_Baro::calibrate [3]\r\n")); } } while (!healthy); - ground_pressure = (ground_pressure * 0.8) + (get_pressure() * 0.2); - ground_temperature = (ground_temperature * 0.8) + - (get_temperature() * 0.2); + ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f); + ground_temperature = (ground_temperature * 0.8f) + + (get_temperature() * 0.2f); hal.scheduler->delay(100); } @@ -111,7 +111,7 @@ float AP_Baro::get_altitude(void) // unsmoothed values scaling = (float)_ground_pressure / (float)get_pressure(); temp = ((float)_ground_temperature) + 273.15f; - _altitude = log(scaling) * temp * 29.271267f; + _altitude = logf(scaling) * temp * 29.271267f; _last_altitude_t = _last_update; @@ -128,6 +128,6 @@ float AP_Baro::get_climb_rate(void) { // we use a 7 point derivative filter on the climb rate. This seems // to produce somewhat reasonable results on real hardware - return _climb_rate_filter.slope() * 1.0e3; + return _climb_rate_filter.slope() * 1.0e3f; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index f0ec79ed00..deb9b5be40 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -379,15 +379,15 @@ void AP_Baro_MS5611::_calculate() // multiple samples, giving us more precision dT = D2-(((uint32_t)C5)<<8); TEMP = (dT * C6)/8388608; - OFF = C2 * 65536.0 + (C4 * dT) / 128; - SENS = C1 * 32768.0 + (C3 * dT) / 256; + OFF = C2 * 65536.0f + (C4 * dT) / 128; + SENS = C1 * 32768.0f + (C3 * dT) / 256; if (TEMP < 0) { // second order temperature compensation when under 20 degrees C float T2 = (dT*dT) / 0x80000000; float Aux = TEMP*TEMP; - float OFF2 = 2.5*Aux; - float SENS2 = 1.25*Aux; + float OFF2 = 2.5f*Aux; + float SENS2 = 1.25f*Aux; TEMP = TEMP - T2; OFF = OFF - OFF2; SENS = SENS - SENS2; diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index c14a9ff1b2..f5d7f22a48 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -221,13 +221,13 @@ AP_Compass_HMC5843::init() float cal[3]; - cal[0] = fabs(expected_x / (float)_mag_x); - cal[1] = fabs(expected_yz / (float)_mag_y); - cal[2] = fabs(expected_yz / (float)_mag_z); + cal[0] = fabsf(expected_x / (float)_mag_x); + cal[1] = fabsf(expected_yz / (float)_mag_y); + cal[2] = fabsf(expected_yz / (float)_mag_z); - if (cal[0] > 0.7 && cal[0] < 1.3 && - cal[1] > 0.7 && cal[1] < 1.3 && - cal[2] > 0.7 && cal[2] < 1.3) { + if (cal[0] > 0.7f && cal[0] < 1.3f && + cal[1] > 0.7f && cal[1] < 1.3f && + cal[2] > 0.7f && cal[2] < 1.3f) { good_count++; calibration[0] += cal[0]; calibration[1] += cal[1]; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 226a4fc6bc..b8bb245fbb 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -144,26 +144,26 @@ Compass::calculate_heading(float roll, float pitch) float sin_pitch; float heading; - cos_roll = cos(roll); - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); + cos_roll = cosf(roll); + sin_roll = sinf(roll); + cos_pitch = cosf(pitch); + sin_pitch = sinf(pitch); // Tilt compensated magnetic field X component: headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch; // Tilt compensated magnetic field Y component: headY = mag_y*cos_roll - mag_z*sin_roll; // magnetic heading - heading = atan2(-headY,headX); + heading = atan2f(-headY,headX); // Declination correction (if supplied) - if( fabs(_declination) > 0.0 ) + if( fabsf(_declination) > 0.0f ) { heading = heading + _declination; - if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) - heading -= (2.0 * M_PI); - else if (heading < -M_PI) - heading += (2.0 * M_PI); + if (heading > PI) // Angle normalization (-180 deg, 180 deg) + heading -= (2.0f * PI); + else if (heading < -PI) + heading += (2.0f * PI); } return heading; @@ -178,11 +178,11 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x)); float heading; - // sin(pitch) = - dcm_matrix(3,1) - // cos(pitch)*sin(roll) = - dcm_matrix(3,2) - // cos(pitch)*cos(roll) = - dcm_matrix(3,3) + // sinf(pitch) = - dcm_matrix(3,1) + // cosf(pitch)*sinf(roll) = - dcm_matrix(3,2) + // cosf(pitch)*cosf(roll) = - dcm_matrix(3,3) - if (cos_pitch == 0.0) { + if (cos_pitch == 0.0f) { // we are pointing straight up or down so don't update our // heading using the compass. Wait for the next iteration when // we hopefully will have valid values again. @@ -195,16 +195,16 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch; // magnetic heading // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S. - heading = constrain(atan2(-headY,headX), -3.15, 3.15); + heading = constrain(atan2f(-headY,headX), -3.15f, 3.15f); // Declination correction (if supplied) - if( fabs(_declination) > 0.0 ) + if( fabsf(_declination) > 0.0f ) { heading = heading + _declination; - if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) - heading -= (2.0 * M_PI); - else if (heading < -M_PI) - heading += (2.0 * M_PI); + if (heading > PI) // Angle normalization (-180 deg, 180 deg) + heading -= (2.0f * PI); + else if (heading < -PI) + heading += (2.0f * PI); } return heading; @@ -254,7 +254,7 @@ Compass::null_offsets(void) for (uint8_t i=0; i<_mag_history_size; i++) { // fill the history buffer with the current mag vector, // with the offset removed - _mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); + _mag_history[i] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z); } _mag_history_index = 0; return; @@ -289,7 +289,7 @@ Compass::null_offsets(void) } // put the vector in the history - _mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); + _mag_history[_mag_history_index] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z); _mag_history_index = (_mag_history_index + 1) % _mag_history_size; // equation 6 of Bills paper diff --git a/libraries/AP_Declination/AP_Declination.cpp b/libraries/AP_Declination/AP_Declination.cpp index a1f422ecba..8d83294f54 100644 --- a/libraries/AP_Declination/AP_Declination.cpp +++ b/libraries/AP_Declination/AP_Declination.cpp @@ -118,8 +118,8 @@ AP_Declination::get_declination(float lat, float lon) lat = constrain(lat, -90, 90); lon = constrain(lon, -180, 180); - latmin = floor(lat/5)*5; - lonmin = floor(lon/5)*5; + latmin = floorf(lat/5)*5; + lonmin = floorf(lon/5)*5; latmin_index= (90+latmin)/5; lonmin_index= (180+lonmin)/5; diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index f51846ff25..24887dfc43 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -34,12 +34,12 @@ void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { time = _time; - latitude = _latitude*1.0e7; - longitude = _longitude*1.0e7; - altitude = _altitude*1.0e2; - ground_speed = _ground_speed*1.0e2; - ground_course = _ground_course*1.0e2; - speed_3d = _speed_3d*1.0e2; + latitude = _latitude*1.0e7f; + longitude = _longitude*1.0e7f; + altitude = _altitude*1.0e2f; + ground_speed = _ground_speed*1.0e2f; + ground_course = _ground_course*1.0e2f; + speed_3d = _speed_3d*1.0e2f; num_sats = _num_sats; fix = true; new_data = true; diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 1d3b878a1c..a1618c108d 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -54,16 +54,16 @@ GPS::update(void) if (_have_raw_velocity) { // the GPS is able to give us velocity numbers directly - _velocity_north = _vel_north * 0.01; - _velocity_east = _vel_east * 0.01; - _velocity_down = _vel_down * 0.01; + _velocity_north = _vel_north * 0.01f; + _velocity_east = _vel_east * 0.01f; + _velocity_down = _vel_down * 0.01f; } else { - float gps_heading = ToRad(ground_course * 0.01); - float gps_speed = ground_speed * 0.01; + float gps_heading = ToRad(ground_course * 0.01f); + float gps_speed = ground_speed * 0.01f; float sin_heading, cos_heading; - cos_heading = cos(gps_heading); - sin_heading = sin(gps_heading); + cos_heading = cosf(gps_heading); + sin_heading = sinf(gps_heading); _velocity_north = gps_speed * cos_heading; _velocity_east = gps_speed * sin_heading; diff --git a/libraries/AP_HAL/utility/Print.cpp b/libraries/AP_HAL/utility/Print.cpp index 18a9310c2d..14ed750c63 100644 --- a/libraries/AP_HAL/utility/Print.cpp +++ b/libraries/AP_HAL/utility/Print.cpp @@ -197,7 +197,7 @@ size_t Print::printFloat(float number, uint8_t digits) size_t n = 0; // Handle negative numbers - if (number < 0.0) + if (number < 0.0f) { n += print('-'); number = -number; @@ -206,7 +206,7 @@ size_t Print::printFloat(float number, uint8_t digits) // Round correctly so that print(1.999, 2) prints as "2.00" float rounding = 0.5; for (uint8_t i=0; i 0) { - remainder *= 10.0; + remainder *= 10.0f; int toPrint = int(remainder); n += print(toPrint); remainder -= toPrint; diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp index e0ba886766..56e3d15503 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -226,7 +226,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, velned.ned_down = 0; velned.speed_2d = pythagorous2(d.speedN, d.speedE) * 100; velned.speed_3d = velned.speed_2d; - velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0; + velned.heading_2d = ToDeg(atan2f(d.speedE, d.speedN)) * 100000.0; if (velned.heading_2d < 0.0) { velned.heading_2d += 360.0 * 100000.0; } diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 3b4600a5cc..779c186a9f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -64,7 +64,7 @@ void AP_InertialNav::update(float dt) Vector3f velocity_increase; // discard samples where dt is too large - if( dt > 0.1 ) { + if( dt > 0.1f ) { return; } @@ -149,7 +149,7 @@ void AP_InertialNav::check_gps() // calculate time since last gps reading now = hal.scheduler->millis(); - float dt = (float)(now - _gps_last_update) / 1000.0; + float dt = (float)(now - _gps_last_update) / 1000.0f; // call position correction method correct_with_gps((*_gps_ptr)->longitude, (*_gps_ptr)->latitude, dt); @@ -167,7 +167,7 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt) float hist_position_base_x, hist_position_base_y; // discard samples where dt is too large - if( dt > 1.0 || dt == 0 || !_xy_enabled) { + if( dt > 1.0f || dt == 0 || !_xy_enabled) { return; } @@ -240,7 +240,7 @@ void AP_InertialNav::set_current_position(int32_t lon, int32_t lat) // set longitude->meters scaling // this is used to offset the shrinking longitude as we go towards the poles - _lon_to_m_scaling = cos((fabs((float)lat)/10000000.0) * 0.0174532925); + _lon_to_m_scaling = cosf((fabsf((float)lat)/10000000.0f) * 0.0174532925f); // reset corrections to base position to zero _position_base.x = 0; @@ -332,7 +332,7 @@ void AP_InertialNav::check_baro() // calculate time since last baro reading baro_update_time = _baro->get_last_update(); if( baro_update_time != _baro_last_update ) { - float dt = (float)(baro_update_time - _baro_last_update) / 1000.0; + float dt = (float)(baro_update_time - _baro_last_update) / 1000.0f; // call correction method correct_with_baro(_baro->get_altitude()*100, dt); _baro_last_update = baro_update_time; @@ -348,7 +348,7 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt) float accel_ef_z_correction; // discard samples where dt is too large - if( dt > 0.5 ) { + if( dt > 0.5f ) { return; } diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 965e5f0534..bbfc520d8e 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -8,9 +8,9 @@ #include // ArduPilot Mega Barometer Library #include // FIFO buffer library -#define AP_INTERTIALNAV_GRAVITY 9.80665 -#define AP_INTERTIALNAV_TC_XY 3.0 // default time constant for complementary filter's X & Y axis -#define AP_INTERTIALNAV_TC_Z 7.0 // default time constant for complementary filter's Z axis +#define AP_INTERTIALNAV_GRAVITY 9.80665f +#define AP_INTERTIALNAV_TC_XY 3.0f // default time constant for complementary filter's X & Y axis +#define AP_INTERTIALNAV_TC_Z 7.0f // default time constant for complementary filter's Z axis #define AP_INTERTIALNAV_ACCEL_CORR_MAX 100.0 // max allowed accelerometer offset correction @@ -19,7 +19,7 @@ #define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10 #define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y -#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175 +#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175f /* * AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e57712a513..6077986e3c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -289,7 +289,7 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) // TO-DO: replace with gravity #define form location.cpp accel_offset.z += GRAVITY; - total_change = fabs(prev.x - accel_offset.x) + fabs(prev.y - accel_offset.y) + fabs(prev.z - accel_offset.z); + total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z); max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y; max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z; @@ -416,7 +416,7 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], // reset beta[0] = beta[1] = beta[2] = 0; - beta[3] = beta[4] = beta[5] = 1.0/GRAVITY; + beta[3] = beta[4] = beta[5] = 1.0f/GRAVITY; while( num_iterations < 20 && change > eps ) { num_iterations++; @@ -454,11 +454,11 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], accel_offsets.z = beta[2] * accel_scale.z; // sanity check scale - if( accel_scale.is_nan() || fabs(accel_scale.x-1.0) > 0.1 || fabs(accel_scale.y-1.0) > 0.1 || fabs(accel_scale.z-1.0) > 0.1 ) { + if( accel_scale.is_nan() || fabsf(accel_scale.x-1.0f) > 0.1f || fabsf(accel_scale.y-1.0f) > 0.1f || fabsf(accel_scale.z-1.0f) > 0.1f ) { success = false; } // sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G) - if( accel_offsets.is_nan() || fabs(accel_offsets.x) > 2.0 || fabs(accel_offsets.y) > 2.0 || fabs(accel_offsets.z) > 3.0 ) { + if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 2.0f || fabsf(accel_offsets.y) > 2.0f || fabsf(accel_offsets.z) > 3.0f ) { success = false; } @@ -478,8 +478,8 @@ void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6], b = beta[3+j]; dx = (float)data[j] - beta[j]; residual -= b*b*dx*dx; - jacobian[j] = 2.0*b*b*dx; - jacobian[3+j] = -2.0*b*dx*dx; + jacobian[j] = 2.0f*b*b*dx; + jacobian[3+j] = -2.0f*b*dx*dx; } for( j=0; j<6; j++ ) { @@ -496,9 +496,9 @@ void AP_InertialSensor::_calibrate_reset_matrices(float dS[6], float JS[6][6]) { int16_t j,k; for( j=0; j<6; j++ ) { - dS[j] = 0.0; + dS[j] = 0.0f; for( k=0; k<6; k++ ) { - JS[j][k] = 0.0; + JS[j][k] = 0.0f; } } } @@ -515,7 +515,7 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float //eliminate all nonzero entries below JS[i][i] for( j=i+1; j<6; j++ ) { mu = JS[i][j]/JS[i][i]; - if( mu != 0.0 ) { + if( mu != 0.0f ) { dS[j] -= mu*dS[i]; for( k=j; k<6; k++ ) { JS[k][j] -= mu*JS[k][i]; @@ -527,12 +527,12 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float //back-substitute for( i=5; i>=0; i-- ) { dS[i] /= JS[i][i]; - JS[i][i] = 1.0; + JS[i][i] = 1.0f; for( j=0; j #include diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 385792cdd4..aeaf521a5f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -6,7 +6,7 @@ extern const AP_HAL::HAL& hal; // MPU6000 accelerometer scaling -#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0) +#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0f) // MPU 6000 registers #define MPUREG_XG_OFFS_TC 0x00 @@ -318,7 +318,7 @@ bool AP_InertialSensor_MPU6000::update( void ) } hal.scheduler->resume_timer_procs(); - count_scale = 1.0 / _num_samples; + count_scale = 1.0f / _num_samples; _gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]], _gyro_data_sign[1] * sum[_gyro_data_index[1]], diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 927e7ec3f0..f4d3bd1b0e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -24,17 +24,17 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900; // Tested value : 418 // Oilpan accelerometer scaling & offsets -#define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0 / (2465.0 - 1617.0)) -#define OILPAN_RAW_ACCEL_OFFSET ((2465.0 + 1617.0) * 0.5) -#define OILPAN_RAW_GYRO_OFFSET 1658.0 +#define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0f / (2465.0f - 1617.0f)) +#define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) +#define OILPAN_RAW_GYRO_OFFSET 1658.0f #define ToRad(x) radians(x) // *pi/180 // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 -const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4); -const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41); -const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41); +const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f); +const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f); /* ------ Public functions -------------------------------------------*/ diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 8c8ea1aab4..3147c9154c 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -8,13 +8,13 @@ float safe_asin(float v) if (isnan(v)) { return 0.0; } - if (v >= 1.0) { + if (v >= 1.0f) { return PI/2; } - if (v <= -1.0) { + if (v <= -1.0f) { return -PI/2; } - return asin(v); + return asinf(v); } // a varient of sqrt() that checks the input ranges and ensures a @@ -24,7 +24,7 @@ float safe_asin(float v) // real input should have been zero float safe_sqrt(float v) { - float ret = sqrt(v); + float ret = sqrtf(v); if (isnan(ret)) { return 0; } @@ -51,7 +51,7 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou tv2(1,2,3); tv2.rotate(r); diff = tv1 - tv2; - if (diff.length() < 1.0e-6) { + if (diff.length() < 1.0e-6f) { // we found a match if (found) { *found = true; @@ -101,10 +101,10 @@ float sq(float v) { // 2D vector length float pythagorous2(float a, float b) { - return sqrt(sq(a)+sq(b)); + return sqrtf(sq(a)+sq(b)); } // 3D vector length float pythagorous3(float a, float b, float c) { - return sqrt(sq(a)+sq(b)+sq(c)); + return sqrtf(sq(a)+sq(b)+sq(c)); } diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 469939043a..19dcdd5a76 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -17,13 +17,13 @@ #include "polygon.h" #ifndef PI -#define PI 3.141592653589793 +#define PI 3.141592653589793f #endif -#define DEG_TO_RAD 0.017453292519943295769236907684886 -#define RAD_TO_DEG 57.295779513082320876798154814105 +#define DEG_TO_RAD 0.017453292519943295769236907684886f +#define RAD_TO_DEG 57.295779513082320876798154814105f // acceleration due to gravity in m/s/s -#define GRAVITY_MSS 9.80665 +#define GRAVITY_MSS 9.80665f #define ROTATION_COMBINATION_SUPPORT 0 diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 31b0657f4a..6f4212a562 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -22,7 +22,7 @@ static float rad_diff(float rad1, float rad2) if (diff < -PI) { diff += 2*PI; } - return fabs(diff); + return fabsf(diff); } static void check_result(float roll, float pitch, float yaw, diff --git a/libraries/AP_Math/examples/location/location.pde b/libraries/AP_Math/examples/location/location.pde index bdb647adc4..17611bcfba 100644 --- a/libraries/AP_Math/examples/location/location.pde +++ b/libraries/AP_Math/examples/location/location.pde @@ -85,7 +85,7 @@ static void test_one_offset(struct Location &loc, brg_error += 360; } - if (fabs(dist - dist2) > 1.0 || + if (fabsf(dist - dist2) > 1.0 || brg_error > 1.0) { hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n", brg_error, dist-dist2); diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 8971b19d60..97b13c4678 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -36,7 +36,7 @@ static float longitude_scale(const struct Location *loc) // the same scale factor. return scale; } - scale = cos((fabs((float)loc->lat)/1.0e7) * 0.0174532925); + scale = cosf((fabsf((float)loc->lat)/1.0e7f) * 0.0174532925f); last_lat = loc->lat; return scale; } @@ -53,7 +53,7 @@ float get_distance(const struct Location *loc1, const struct Location *loc2) return -1; float dlat = (float)(loc2->lat - loc1->lat); float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2); - return pythagorous2(dlat, dlong) * 0.01113195; + return pythagorous2(dlat, dlong) * 0.01113195f; } // return distance in centimeters to between two locations, or -1 if @@ -68,7 +68,7 @@ int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2) { int32_t off_x = loc2->lng - loc1->lng; int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2); - int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f; if (bearing < 0) bearing += 36000; return bearing; } @@ -120,17 +120,17 @@ bool location_passed_point(struct Location &location, */ void location_update(struct Location *loc, float bearing, float distance) { - float lat1 = radians(loc->lat*1.0e-7); - float lon1 = radians(loc->lng*1.0e-7); + float lat1 = radians(loc->lat*1.0e-7f); + float lon1 = radians(loc->lng*1.0e-7f); float brng = radians(bearing); float dr = distance/RADIUS_OF_EARTH; - float lat2 = asin(sin(lat1)*cos(dr) + - cos(lat1)*sin(dr)*cos(brng)); - float lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), - cos(dr)-sin(lat1)*sin(lat2)); - loc->lat = degrees(lat2)*1.0e7; - loc->lng = degrees(lon2)*1.0e7; + float lat2 = asinf(sinf(lat1)*cosf(dr) + + cosf(lat1)*sinf(dr)*cosf(brng)); + float lon2 = lon1 + atan2f(sinf(brng)*sinf(dr)*cosf(lat1), + cosf(dr)-sinf(lat1)*sinf(lat2)); + loc->lat = degrees(lat2)*1.0e7f; + loc->lng = degrees(lon2)*1.0e7f; } /* @@ -140,8 +140,8 @@ void location_update(struct Location *loc, float bearing, float distance) void location_offset(struct Location *loc, float ofs_north, float ofs_east) { if (ofs_north != 0 || ofs_east != 0) { - float dlat = ofs_north * 89.831520982; - float dlng = (ofs_east * 89.831520982) / longitude_scale(loc); + float dlat = ofs_north * 89.831520982f; + float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc); loc->lat += dlat; loc->lng += dlng; } diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 063c3a6060..ed790d198e 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -140,12 +140,12 @@ void Matrix3::rotation(enum Rotation r) template void Matrix3::from_euler(float roll, float pitch, float yaw) { - float cp = cos(pitch); - float sp = sin(pitch); - float sr = sin(roll); - float cr = cos(roll); - float sy = sin(yaw); - float cy = cos(yaw); + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); a.x = cp * cy; a.y = (sr * sp * cy) - (cr * sy); @@ -167,10 +167,10 @@ void Matrix3::to_euler(float *roll, float *pitch, float *yaw) *pitch = -safe_asin(c.x); } if (roll != NULL) { - *roll = atan2(c.y, c.z); + *roll = atan2f(c.y, c.z); } if (yaw != NULL) { - *yaw = atan2(b.x, a.x); + *yaw = atan2f(b.x, a.x); } } diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 4bfa09e018..a7443b82f7 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -58,12 +58,12 @@ void Quaternion::earth_to_body(Vector3f &v) // create a quaternion from Euler angles void Quaternion::from_euler(float roll, float pitch, float yaw) { - float cr2 = cos(roll*0.5); - float cp2 = cos(pitch*0.5); - float cy2 = cos(yaw*0.5); - float sr2 = sin(roll*0.5); - float sp2 = sin(pitch*0.5); - float sy2 = sin(yaw*0.5); + float cr2 = cosf(roll*0.5f); + float cp2 = cosf(pitch*0.5f); + float cy2 = cosf(yaw*0.5f); + float sr2 = sinf(roll*0.5f); + float sp2 = sinf(pitch*0.5f); + float sy2 = sinf(yaw*0.5f); q1 = cr2*cp2*cy2 + sr2*sp2*sy2; q2 = sr2*cp2*cy2 - cr2*sp2*sy2; @@ -75,15 +75,15 @@ void Quaternion::from_euler(float roll, float pitch, float yaw) void Quaternion::to_euler(float *roll, float *pitch, float *yaw) { if (roll) { - *roll = (atan2(2.0*(q1*q2 + q3*q4), - 1 - 2.0*(q2*q2 + q3*q3))); + *roll = (atan2f(2.0f*(q1*q2 + q3*q4), + 1 - 2.0f*(q2*q2 + q3*q3))); } if (pitch) { // we let safe_asin() handle the singularities near 90/-90 in pitch - *pitch = safe_asin(2.0*(q1*q3 - q4*q2)); + *pitch = safe_asin(2.0f*(q1*q3 - q4*q2)); } if (yaw) { - *yaw = atan2(2.0*(q1*q4 + q2*q3), - 1 - 2.0*(q3*q3 + q4*q4)); + *yaw = atan2f(2.0f*(q1*q4 + q2*q3), + 1 - 2.0f*(q3*q3 + q4*q4)); } } diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 3c4d0317f2..0514f492a7 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -167,19 +167,19 @@ struct Vector2 // computes the angle between 2 arbitrary vectors T angle(const Vector2 &v1, const Vector2 &v2) { - return (T)acos((v1*v2) / (v1.length()*v2.length())); + return (T)acosf((v1*v2) / (v1.length()*v2.length())); } // computes the angle between this vector and another vector T angle(const Vector2 &v2) { - return (T)acos(((*this)*v2) / (this->length()*v2.length())); + return (T)acosf(((*this)*v2) / (this->length()*v2.length())); } // computes the angle between 2 normalized arbitrary vectors T angle_normalized(const Vector2 &v1, const Vector2 &v2) { - return (T)acos(v1*v2); + return (T)acosf(v1*v2); } }; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index e9613c8512..c6ce8ac850 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -19,7 +19,7 @@ #include "AP_Math.h" -#define HALF_SQRT_2 0.70710678118654757 +#define HALF_SQRT_2 0.70710678118654757f // rotate a vector by a standard rotation, attempting // to use the minimum number of floating point operations diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 4da8dd8a0c..15f051093c 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -193,19 +193,19 @@ public: // computes the angle between 2 arbitrary vectors T angle(const Vector3 &v1, const Vector3 &v2) { - return (T)acos((v1*v2) / (v1.length()*v2.length())); + return (T)acosf((v1*v2) / (v1.length()*v2.length())); } // computes the angle between this vector and another vector T angle(const Vector3 &v2) { - return (T)acos(((*this)*v2) / (this->length()*v2.length())); + return (T)acosf(((*this)*v2) / (this->length()*v2.length())); } // computes the angle between 2 arbitrary normalized vectors T angle_normalized(const Vector3 &v1, const Vector3 &v2) { - return (T)acos(v1*v2); + return (T)acosf(v1*v2); } // check if any elements are NAN diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 405069f826..5d853884f7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -330,14 +330,14 @@ void AP_MotorsHeli::reset_swash() if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing // roll factors - _rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle)); - _rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle)); - _rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle)); + _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle)); + _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle)); + _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle)); // pitch factors - _pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle)); - _pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle)); - _pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle)); + _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle)); + _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle)); + _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle)); // collective factors _collectiveFactor[CH_1] = 1; @@ -363,10 +363,10 @@ void AP_MotorsHeli::reset_swash() } // set roll, pitch and throttle scaling - _roll_scaler = 1.0; - _pitch_scaler = 1.0; - _collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0; - _stab_throttle_scalar = 1.0; + _roll_scaler = 1.0f; + _pitch_scaler = 1.0f; + _collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0f; + _stab_throttle_scalar = 1.0f; // we must be in set-up mode so mark swash as uninitialised _swash_initialised = false; @@ -391,25 +391,25 @@ void AP_MotorsHeli::init_swash() collective_mid = constrain(collective_mid, collective_min, collective_max); // calculate throttle mid point - throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0; + throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0f; // determine roll, pitch and throttle scaling - _roll_scaler = (float)roll_max/4500.0; - _pitch_scaler = (float)pitch_max/4500.0; - _collective_scalar = ((float)(collective_max-collective_min))/1000.0; - _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0; + _roll_scaler = (float)roll_max/4500.0f; + _pitch_scaler = (float)pitch_max/4500.0f; + _collective_scalar = ((float)(collective_max-collective_min))/1000.0f; + _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0f; if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing // roll factors - _rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle)); - _rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle)); - _rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle)); + _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle)); + _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle)); + _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle)); // pitch factors - _pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle)); - _pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle)); - _pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle)); + _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle)); + _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle)); + _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle)); // collective factors _collectiveFactor[CH_1] = 1; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index f7215b651c..94258b130b 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -353,8 +353,8 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t di // call raw motor set-up method add_motor_raw( motor_num, - cos(radians(angle_degrees + 90)), // roll factor - cos(radians(angle_degrees)), // pitch factor + cosf(radians(angle_degrees + 90)), // roll factor + cosf(radians(angle_degrees)), // pitch factor (float)direction, // yaw factor testing_order); diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 44f78eb253..79a31f8f9d 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -80,7 +80,7 @@ void AP_MotorsTri::output_armed() _rc_throttle->calc_pwm(); _rc_yaw->calc_pwm(); - int roll_out = (float)_rc_roll->pwm_out * .866; + int roll_out = (float)_rc_roll->pwm_out * 0.866f; int pitch_out = _rc_pitch->pwm_out / 2; //left front diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 7a8b61cbeb..6ff70e3355 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -97,8 +97,8 @@ bool AP_Motors::setup_throttle_curve() int16_t min_pwm = _rc_throttle->radio_min; int16_t max_pwm = _rc_throttle->radio_max; int16_t mid_throttle_pwm = (max_pwm + min_pwm) / 2; - int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0); - int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0); + int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0f); + int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0f); bool retval = true; // some basic checks that the curve is valid diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c2c2d6643b..a659611a71 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -346,21 +346,21 @@ void AP_Mount::update_mount_position() // allow pilot speed position input to come directly from an RC_Channel if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { //_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed; - _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; - if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01); - if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01); + _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001f * _joystick_speed; + if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f); + if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f); } if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { //_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed; - _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; - if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01); - if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01); + _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001f * _joystick_speed; + if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f); + if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f); } if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { //_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed; - _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; - if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01); - if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01); + _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001f * _joystick_speed; + if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f); + if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f); } } else { #endif @@ -408,9 +408,9 @@ void AP_Mount::update_mount_position() #endif // write the results to the servos - move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); - move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); - move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); + move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1f, _roll_angle_max*0.1f); + move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f); + move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1f, _pan_angle_max*0.1f); } void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) @@ -449,7 +449,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) { #if MNT_RETRACT_OPTION == ENABLED case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization - set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); + set_retract_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); if (packet.save_position) { _retract_angles.save(); @@ -458,7 +458,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) #endif case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM - set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); + set_neutral_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); if (packet.save_position) { _neutral_angles.save(); @@ -466,7 +466,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) break; case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization - set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); + set_control_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); break; case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization @@ -570,19 +570,19 @@ AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) float AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) { - return radians(angle_input(rc, angle_min, angle_max)*0.01); + return radians(angle_input(rc, angle_min, angle_max)*0.01f); } void AP_Mount::calc_GPS_target_angle(struct Location *target) { - float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.01113195; - float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195; + float GPS_vector_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f; + float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f; float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). - float target_distance = 100.0*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. + float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. _roll_control_angle = 0; - _tilt_control_angle = atan2(GPS_vector_z, target_distance); - _pan_control_angle = atan2(GPS_vector_x, GPS_vector_y); + _tilt_control_angle = atan2f(GPS_vector_z, target_distance); + _pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y); } /// Stabilizes mount relative to the Earth's frame diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 4bed2a5480..9d3e2b4d1e 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -10,7 +10,7 @@ #include "AP_OpticalFlow.h" -#define FORTYFIVE_DEGREES 0.78539816 +#define FORTYFIVE_DEGREES 0.78539816f // pointer to the last instantiated optical flow sensor. Will be turned into // a table if we ever add support for more than one sensor @@ -81,8 +81,8 @@ void AP_OpticalFlow::update_conversion_factors() { // multiply this number by altitude and pixel change to get horizontal // move (in same units as altitude) - conv_factor = ((1.0 / (float)(num_pixels * scaler)) - * 2.0 * tan(field_of_view / 2.0)); + conv_factor = ((1.0f / (float)(num_pixels * scaler)) + * 2.0f * tanf(field_of_view / 2.0f)); // 0.00615 radians_to_pixels = (num_pixels * scaler) / field_of_view; // 162.99 @@ -97,8 +97,8 @@ void AP_OpticalFlow::update_position(float roll, float pitch, // only update position if surface quality is good and angle is not // over 45 degrees - if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES - && fabs(pitch) <= FORTYFIVE_DEGREES ) { + if( surface_quality >= 10 && fabsf(roll) <= FORTYFIVE_DEGREES + && fabsf(pitch) <= FORTYFIVE_DEGREES ) { altitude = max(altitude, 0); // calculate expected x,y diff due to roll and pitch change exp_change_x = diff_roll * radians_to_pixels; @@ -108,7 +108,7 @@ void AP_OpticalFlow::update_position(float roll, float pitch, change_x = dx - exp_change_x; change_y = dy - exp_change_y; - float avg_altitude = (altitude + _last_altitude)*0.5; + float avg_altitude = (altitude + _last_altitude)*0.5f; // convert raw change to horizontal movement in cm // perhaps this altitude should actually be the distance to the diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 93a5a1ecd5..6b5d4083e0 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -696,7 +696,7 @@ bool AP_Param::save(void) return true; } if (phdr.type != AP_PARAM_INT32 && - (fabs(v1-v2) < 0.0001*fabs(v1))) { + (fabsf(v1-v2) < 0.0001f*fabsf(v1))) { // for other than 32 bit integers, we accept values within // 0.01 percent of the current value as being the same return true; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 4618dcac44..f9f823e806 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -40,7 +40,7 @@ AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_HAL::AnalogSource *sourc // Public Methods ////////////////////////////////////////////////////////////// float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) { - float type_scaler = 1.0; + float type_scaler = 1.0f; switch(sonar_type) { case AP_RANGEFINDER_MAXSONARXL: type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; @@ -63,6 +63,6 @@ float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refe max_distance = AP_RANGEFINDER_MAXSONARHRLV_MAX_DISTANCE; break; } - _scaler = type_scaler * adc_refence_voltage / 5.0; + _scaler = type_scaler * adc_refence_voltage / 5.0f; return _scaler; } diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h index 09fb1601a1..8b9c675e9e 100644 --- a/libraries/Filter/LowPassFilter.h +++ b/libraries/Filter/LowPassFilter.h @@ -78,7 +78,7 @@ template void LowPassFilter::set_cutoff_frequency(float time_step, float cutoff_freq) { // calculate alpha - float rc = 1/(2*(float)M_PI*cutoff_freq); + float rc = 1/(2*PI*cutoff_freq); _alpha = time_step / (time_step + rc); } diff --git a/libraries/Filter/examples/Derivative/Derivative.pde b/libraries/Filter/examples/Derivative/Derivative.pde index a10ee2330a..ebd0b9cff0 100644 --- a/libraries/Filter/examples/Derivative/Derivative.pde +++ b/libraries/Filter/examples/Derivative/Derivative.pde @@ -21,7 +21,7 @@ void setup(){} static float noise(void) { - return ((random() % 100)-50) * 0.001; + return ((random() % 100)-50) * 0.001f; } //Main loop where the action takes place @@ -29,12 +29,12 @@ void loop() { hal.scheduler->delay(50); float t = hal.scheduler->millis()*1.0e-3; - float s = sin(t); + float s = sinf(t); //s += noise(); uint32_t t1 = hal.scheduler->micros(); derivative.update(s, t1); - float output = derivative.slope() * 1.0e6; - hal.console->printf("%f %f %f %f\n", t, output, s, cos(t)); + float output = derivative.slope() * 1.0e6f; + hal.console->printf("%f %f %f %f\n", t, output, s, cosf(t)); } AP_HAL_MAIN(); diff --git a/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde index 9e70558e55..a3e2dcfd5b 100644 --- a/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde +++ b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde @@ -24,8 +24,8 @@ void setup() hal.console->printf("ArduPilot LowPassFilter test ver 1.0\n\n"); // set-up filter - low_pass_filter.set_time_constant(0.02, 0.015); - //low_pass_filter.set_cutoff_frequency(0.02, 1.0); + low_pass_filter.set_time_constant(0.02f, 0.015f); + //low_pass_filter.set_cutoff_frequency(0.02f, 1.0f); // Wait for the serial connection hal.scheduler->delay(500); @@ -44,7 +44,7 @@ void loop() for( i=0; i<300; i++ ) { // new data value - new_value = sin((float)i*2*M_PI*5/50.0); // 5hz + new_value = sinf((float)i*2*PI*5/50.0f); // 5hz // output to user hal.console->printf("applying: %6.4f", new_value); diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 9bf4812313..eafc0d8d72 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -7,6 +7,7 @@ #include "PID.h" #include +#include extern const AP_HAL::HAL& hal; @@ -36,13 +37,13 @@ int32_t PID::get_pid(int32_t error, float scaler) } _last_t = tnow; - delta_time = (float)dt / 1000.0; + delta_time = (float)dt / 1000.0f; // Compute proportional component output += error * _kp; // Compute derivative component if time has elapsed - if ((fabs(_kd) > 0) && (dt > 0)) { + if ((fabsf(_kd) > 0) && (dt > 0)) { float derivative; if (isnan(_last_derivative)) { @@ -57,7 +58,7 @@ int32_t PID::get_pid(int32_t error, float scaler) // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy - float RC = 1/(2*M_PI*_fCut); + float RC = 1/(2*PI*_fCut); derivative = _last_derivative + ((delta_time / (RC + delta_time)) * (derivative - _last_derivative)); @@ -74,7 +75,7 @@ int32_t PID::get_pid(int32_t error, float scaler) output *= scaler; // Compute integral component if time has elapsed - if ((fabs(_ki) > 0) && (dt > 0)) { + if ((fabsf(_ki) > 0) && (dt > 0)) { _integrator += (error * _ki) * scaler * delta_time; if (_integrator < -_imax) { _integrator = -_imax; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 70f6b07c16..cc591e5c24 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -180,7 +180,7 @@ RC_Channel::calc_pwm(void) radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out); }else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) { - pwm_out = (float)servo_out * .1; + pwm_out = (float)servo_out * 0.1f; radio_out = (pwm_out * _reverse) + radio_trim; }else{ // RC_CHANNEL_TYPE_ANGLE