fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 03:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

avoid double promotions

Browse Source
This commit is contained in:
Daniel Agar 2017-03-18 22:00:29 -04:00 committed by Lorenz Meier
parent 412f956636
commit 4b8bedef48
12 changed files with 53 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/examples/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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 <stdarg.h>
#include <px4_defines.h>
#include <mathlib/mathlib.h>
#include <cmath>
#ifndef M_PI_F
#define M_PI_F static_cast<float>(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];
}

2
src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -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()

12
src/lib/geo/geo.c
View File

@ -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;

2
src/lib/matrix

@ -1 +1 @@
Subproject commit 499b897e5f270c3207a0e88d2f7239c5885d1681
Subproject commit 471e96ff6f5f22018b782441c6a8df19d8294181

2
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -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

2
src/modules/controllib_test/controllib_test_main.cpp
View File

@ -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));

1
src/modules/fw_pos_control_l1/CMakeLists.txt
View File

@ -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

28
src/modules/sensors/parameters.cpp
View File

@ -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);

2
src/modules/unit_test/unit_test.h
View File

@ -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) { \

11
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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");
}
}

12
src/systemcmds/tests/test_float.cpp
View File

@ -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 <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <cfloat>
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);

7
src/systemcmds/tests/test_matrix.cpp
View File

@ -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);