diff --git a/src/examples/ekf_att_pos_estimator/estimator_22states.cpp b/src/examples/ekf_att_pos_estimator/estimator_22states.cpp index 4e5b70f439..86168bc361 100644 --- a/src/examples/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/examples/ekf_att_pos_estimator/estimator_22states.cpp @@ -37,16 +37,14 @@ * @author Lorenz Meier */ -#include #include "estimator_22states.h" -#include -#include -#include + +#include +#include #include -#ifndef M_PI_F -#define M_PI_F static_cast(M_PI) -#endif +using std::cos; +using std::sin; #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[1][1] = fmax(nextPopt[1][1],0.0f); + Popt[0][0] = math::max(nextPopt[0][0],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[1][1] = fmax(nextPopt[1][1],0.0f); + Popt[0][0] = math::max(nextPopt[0][0],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]; } diff --git a/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp b/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp index 16c4150930..13ba3b205a 100644 --- a/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/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() diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 94f450291e..ebf73e6803 100644 --- a/src/lib/geo/geo.c +++ b/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_y = (double)radius * cos(arc_start_bearing); - double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); - double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double start_disp_x = (double)radius * sin((double)arc_start_bearing); + double start_disp_y = (double)radius * cos((double)arc_start_bearing); + double end_disp_x = (double)radius * sin((double)_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; diff --git a/src/lib/matrix b/src/lib/matrix index 499b897e5f..471e96ff6f 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 499b897e5f270c3207a0e88d2f7239c5885d1681 +Subproject commit 471e96ff6f5f22018b782441c6a8df19d8294181 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 29c2615d2a..0fc9b159f0 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/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 diff --git a/src/modules/controllib_test/controllib_test_main.cpp b/src/modules/controllib_test/controllib_test_main.cpp index a9be18848f..15549a16f2 100644 --- a/src/modules/controllib_test/controllib_test_main.cpp +++ b/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)); diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 094cc4bc80..3460a395bf 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/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 diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index b49dfa06d7..f586076726 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -366,46 +366,46 @@ int update_parameters(const ParameterHandles ¶meter_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); diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 561597ea14..2a6150d4c4 100644 --- a/src/modules/unit_test/unit_test.h +++ b/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) { \ diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index aabc53eccc..cac0ac2c0d 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -40,11 +40,12 @@ */ #include "vtol_type.h" -#include "drivers/drv_pwm_output.h" -#include -#include #include "vtol_att_control_main.h" +#include +#include +#include + 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"); } } diff --git a/src/systemcmds/tests/test_float.cpp b/src/systemcmds/tests/test_float.cpp index 70fe251804..50b5854d7d 100644 --- a/src/systemcmds/tests/test_float.cpp +++ b/src/systemcmds/tests/test_float.cpp @@ -1,15 +1,7 @@ #include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include +#include 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); diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index 029f36ceb5..eef8451cd3 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/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 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);