avoid double promotions

src/examples/ekf_att_pos_estimator/estimator_22states.cpp

@@ -37,16 +37,14 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
-#include <px4_defines.h>
 #include "estimator_22states.h"
-#include <string.h>
+
-#include <stdio.h>
+#include <px4_defines.h>
-#include <stdarg.h>
+#include <mathlib/mathlib.h>
 #include <cmath>
 
-#ifndef M_PI_F
+using std::cos;
-#define M_PI_F static_cast<float>(M_PI)
+using std::sin;
-#endif
 
 #define MIN_AIRSPEED_MEAS 5.0f
 
@@ -1855,7 +1853,7 @@ void AttPosEKF::FuseOptFlow()
     Vector3f relVelSensor;
 
     // Perform sequential fusion of optical flow measurements only with valid tilt and height
-    flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+    flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
     float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
     bool validTilt = Tnb.z.z > 0.71f;
     if (validTilt)
@@ -2114,7 +2112,7 @@ void AttPosEKF::OpticalFlowEKF()
         } else {
             return;
         }
-        distanceTravelledSq = fmin(distanceTravelledSq, 100.0f);
+        distanceTravelledSq = math::min(distanceTravelledSq, 100.0f);
         Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
     }
 
@@ -2154,7 +2152,7 @@ void AttPosEKF::OpticalFlowEKF()
         varInnovRng = 1.0f/SK_RNG[1];
 
         // constrain terrain height to be below the vehicle
-        flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
+        flowStates[1] = math::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
         // estimate range to centre of image
         range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@@ -2174,7 +2172,7 @@ void AttPosEKF::OpticalFlowEKF()
             }
             // constrain the states
             flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
-            flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
+            flowStates[1] = math::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
             // correct the covariance matrix
             float nextPopt[2][2];
@@ -2183,8 +2181,8 @@ void AttPosEKF::OpticalFlowEKF()
             nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
             nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
             // prevent the state variances from becoming negative and maintain symmetry
-            Popt[0][0] = fmax(nextPopt[0][0],0.0f);
+            Popt[0][0] = math::max(nextPopt[0][0],0.0f);
-            Popt[1][1] = fmax(nextPopt[1][1],0.0f);
+            Popt[1][1] = math::max(nextPopt[1][1],0.0f);
             Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
             Popt[1][0] = Popt[0][1];
         }
@@ -2223,7 +2221,7 @@ void AttPosEKF::OpticalFlowEKF()
         vel.z          = statesAtFlowTime[6];
 
         // constrain terrain height to be below the vehicle
-        flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+        flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
 
         // estimate range to centre of image
         range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@@ -2291,7 +2289,7 @@ void AttPosEKF::OpticalFlowEKF()
                 }
                 // constrain the states
                 flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
-                flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+                flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
 
                 // correct the covariance matrix
                 for (uint8_t i = 0; i < 2 ; i++) {
@@ -2307,8 +2305,8 @@ void AttPosEKF::OpticalFlowEKF()
                 }
 
                 // prevent the state variances from becoming negative and maintain symmetry
-                Popt[0][0] = fmax(nextPopt[0][0],0.0f);
+                Popt[0][0] = math::max(nextPopt[0][0],0.0f);
-                Popt[1][1] = fmax(nextPopt[1][1],0.0f);
+                Popt[1][1] = math::max(nextPopt[1][1],0.0f);
                 Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
                 Popt[1][0] = Popt[0][1];
             }

src/examples/ekf_att_pos_estimator/estimator_utilities.cpp

@@ -76,7 +76,7 @@ void swap_var(float &d1, float &d2);
 
 float Vector3f::length() const
 {
-    return sqrt(x*x + y*y + z*z);
+    return sqrtf(x*x + y*y + z*z);
 }
 
 void Vector3f::zero()

src/lib/geo/geo.c

@@ -319,7 +319,7 @@ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_st
 		double *lat_target, double *lon_target)
 {
 	bearing = _wrap_2pi(bearing);
-	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
+	double radius_ratio = fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH;
 
 	double lat_start_rad = lat_start * M_DEG_TO_RAD;
 	double lon_start_rad = lon_start * M_DEG_TO_RAD;
@@ -432,7 +432,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
 
 	crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
 
-	if (sin(bearing_diff) >= 0) {
+	if (sinf(bearing_diff) >= 0) {
 		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
 
 	} else {
@@ -516,10 +516,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 		// calculate the position of the start and end points.  We should not be doing this often
 		// as this function generally will not be called repeatedly when we are out of the sector.
 
-		double start_disp_x = (double)radius * sin(arc_start_bearing);
+		double start_disp_x = (double)radius * sin((double)arc_start_bearing);
-		double start_disp_y = (double)radius * cos(arc_start_bearing);
+		double start_disp_y = (double)radius * cos((double)arc_start_bearing);
-		double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_x = (double)radius * sin((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
-		double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_y = (double)radius * cos((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
 		double lon_start = lon_now + start_disp_x / 111111.0;
 		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
 		double lon_end = lon_now + end_disp_x / 111111.0;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -675,7 +675,7 @@ bool AttitudeEstimatorQ::update(float dt)
 		const float fifty_dps = 0.873f;
 
 		if (spinRate > fifty_dps) {
-			gainMult = fmin(spinRate / fifty_dps, 10.0f);
+			gainMult = math::min(spinRate / fifty_dps, 10.0f);
 		}
 
 		// Project magnetometer correction to body frame

src/modules/controllib_test/controllib_test_main.cpp

@@ -475,7 +475,7 @@ int blockRandGaussTest()
 		mean = newMean;
 	}
 
-	float stdDev = sqrt(sum / (n - 1));
+	float stdDev = sqrtf(sum / (n - 1));
 	(void)(stdDev);
 	ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
 	ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN fw_pos_control_l1
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Weffc++
 	SRCS
 		fw_pos_control_l1_main.cpp
 		landingslope.cpp

src/modules/sensors/parameters.cpp

@@ -366,46 +366,46 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
 	param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr));
 	param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th));
 	parameters.rc_assist_inv = (parameters.rc_assist_th < 0);
-	parameters.rc_assist_th = fabs(parameters.rc_assist_th);
+	parameters.rc_assist_th = fabsf(parameters.rc_assist_th);
 	param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th));
 	parameters.rc_auto_inv = (parameters.rc_auto_th < 0);
-	parameters.rc_auto_th = fabs(parameters.rc_auto_th);
+	parameters.rc_auto_th = fabsf(parameters.rc_auto_th);
 	param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th));
 	parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0);
-	parameters.rc_rattitude_th = fabs(parameters.rc_rattitude_th);
+	parameters.rc_rattitude_th = fabsf(parameters.rc_rattitude_th);
 	param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th));
 	parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0);
-	parameters.rc_posctl_th = fabs(parameters.rc_posctl_th);
+	parameters.rc_posctl_th = fabsf(parameters.rc_posctl_th);
 	param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th));
 	parameters.rc_return_inv = (parameters.rc_return_th < 0);
-	parameters.rc_return_th = fabs(parameters.rc_return_th);
+	parameters.rc_return_th = fabsf(parameters.rc_return_th);
 	param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th));
 	parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0);
-	parameters.rc_loiter_th = fabs(parameters.rc_loiter_th);
+	parameters.rc_loiter_th = fabsf(parameters.rc_loiter_th);
 	param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th));
 	parameters.rc_acro_inv = (parameters.rc_acro_th < 0);
-	parameters.rc_acro_th = fabs(parameters.rc_acro_th);
+	parameters.rc_acro_th = fabsf(parameters.rc_acro_th);
 	param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th));
 	parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0);
-	parameters.rc_offboard_th = fabs(parameters.rc_offboard_th);
+	parameters.rc_offboard_th = fabsf(parameters.rc_offboard_th);
 	param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th));
 	parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0);
-	parameters.rc_killswitch_th = fabs(parameters.rc_killswitch_th);
+	parameters.rc_killswitch_th = fabsf(parameters.rc_killswitch_th);
 	param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th));
 	parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0);
-	parameters.rc_armswitch_th = fabs(parameters.rc_armswitch_th);
+	parameters.rc_armswitch_th = fabsf(parameters.rc_armswitch_th);
 	param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th));
 	parameters.rc_trans_inv = (parameters.rc_trans_th < 0);
-	parameters.rc_trans_th = fabs(parameters.rc_trans_th);
+	parameters.rc_trans_th = fabsf(parameters.rc_trans_th);
 	param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th));
 	parameters.rc_gear_inv = (parameters.rc_gear_th < 0);
-	parameters.rc_gear_th = fabs(parameters.rc_gear_th);
+	parameters.rc_gear_th = fabsf(parameters.rc_gear_th);
 	param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th));
 	parameters.rc_stab_inv = (parameters.rc_stab_th < 0);
-	parameters.rc_stab_th = fabs(parameters.rc_stab_th);
+	parameters.rc_stab_th = fabsf(parameters.rc_stab_th);
 	param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th));
 	parameters.rc_man_inv = (parameters.rc_man_th < 0);
-	parameters.rc_man_th = fabs(parameters.rc_man_th);
+	parameters.rc_man_th = fabsf(parameters.rc_man_th);
 
 	param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate));
 	parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate);

src/modules/unit_test/unit_test.h

@@ -147,7 +147,7 @@ protected:
 /// since it will give you better error reporting of the actual values being compared.
 #define ut_compare_float(message, v1, v2, precision)						\
 	do {											\
-		int _p = pow(10, precision);							\
+		int _p = pow(10.0f, precision);							\
 		int _v1 = (int)(v1 * _p + 0.5f);						\
 		int _v2 = (int)(v2 * _p + 0.5f);						\
 		if (_v1 != _v2) {								\

src/modules/vtol_att_control/vtol_type.cpp

@@ -40,11 +40,12 @@
 */
 
 #include "vtol_type.h"
-#include "drivers/drv_pwm_output.h"
-#include <px4_defines.h>
-#include <float.h>
 #include "vtol_att_control_main.h"
 
+#include <cfloat>
+#include <px4_defines.h>
+#include <matrix/math.hpp>
+
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_attc(att_controller),
 	_vtol_mode(ROTARY_WING)
@@ -209,7 +210,7 @@ void VtolType::check_quadchute_condition()
 		// fixed-wing maximum pitch angle
 		if (_params->fw_qc_max_pitch > 0) {
 
-			if (fabs(euler.theta()) > fabs(math::radians(_params->fw_qc_max_pitch))) {
+			if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
 				_attc->abort_front_transition("Maximum pitch angle exceeded");
 			}
 		}
@@ -217,7 +218,7 @@ void VtolType::check_quadchute_condition()
 		// fixed-wing maximum roll angle
 		if (_params->fw_qc_max_roll > 0) {
 
-			if (fabs(euler.phi()) > fabs(math::radians(_params->fw_qc_max_roll))) {
+			if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
 				_attc->abort_front_transition("Maximum roll angle exceeded");
 			}
 		}

src/systemcmds/tests/test_float.cpp

@@ -1,15 +1,7 @@
 #include <unit_test/unit_test.h>
 
-#include <errno.h>
-#include <fcntl.h>
-#include <float.h>
-#include <math.h>
 #include <px4_config.h>
-#include <stdint.h>
+#include <cfloat>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <unistd.h>
 
 typedef union {
 	float f;
@@ -33,7 +25,7 @@ bool FloatTest::singlePrecisionTests()
 {
 	float sinf_zero = sinf(0.0f);
 	float sinf_one = sinf(1.0f);
-	float sqrt_two = sqrt(2.0f);
+	float sqrt_two = sqrtf(2.0f);
 
 	ut_assert("sinf(0.0f) == 0.0f", fabsf(sinf_zero) < FLT_EPSILON);
 	ut_assert("sinf(1.0f) == 0.84147f", fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON);

src/systemcmds/tests/test_matrix.cpp

@@ -64,9 +64,11 @@ using matrix::Quatf;
 using matrix::Eulerf;
 using matrix::Vector3f;
 
+using std::fabs;
+
 bool MatrixTest::attitudeTests()
 {
-	double eps = 1e-6;
+	float eps = 1e-6;
 
 	// check data
 	Eulerf euler_check(0.1f, 0.2f, 0.3f);
@@ -207,8 +209,9 @@ bool MatrixTest::attitudeTests()
 	Quatf q_from_m(m4);
 	ut_test(isEqual(q_from_m, m4));
 
-	// quaternion derivate
+	// quaternion derivative
 	Vector<float, 4> q_dot = q.derivative1(Vector3f(1, 2, 3));
+	(void)q_dot;
 
 	// quaternion product
 	Quatf q_prod_check(0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);