AP_Math.cpp File Reference

#include "AP_Math.h"
#include <float.h>

Include dependency graph for AP_Math.cpp:

Go to the source code of this file.

Functions
template<typename Arithmetic1 , typename Arithmetic2 >
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value,bool >::type	is_equal (const Arithmetic1 v_1, const Arithmetic2 v_2)
template<typename Arithmetic1 , typename Arithmetic2 >
std::enable_if< std::is_floating_point< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value, bool >::type	is_equal (const Arithmetic1 v_1, const Arithmetic2 v_2)
template bool	is_equal< int > (const int v_1, const int v_2)
template bool	is_equal< short > (const short v_1, const short v_2)
template bool	is_equal< long > (const long v_1, const long v_2)
template bool	is_equal< float > (const float v_1, const float v_2)
template bool	is_equal< double > (const double v_1, const double v_2)
template<typename T >
float	safe_asin (const T v)
template float	safe_asin< int > (const int v)
template float	safe_asin< short > (const short v)
template float	safe_asin< float > (const float v)
template float	safe_asin< double > (const double v)
template<typename T >
float	safe_sqrt (const T v)
template float	safe_sqrt< int > (const int v)
template float	safe_sqrt< short > (const short v)
template float	safe_sqrt< float > (const float v)
template float	safe_sqrt< double > (const double v)
float	linear_interpolate (float low_output, float high_output, float var_value, float var_low, float var_high)
template<typename T >
float	wrap_180 (const T angle, float unit_mod)
template float	wrap_180< int > (const int angle, float unit_mod)
template float	wrap_180< short > (const short angle, float unit_mod)
template float	wrap_180< float > (const float angle, float unit_mod)
template float	wrap_180< double > (const double angle, float unit_mod)
template<typename T >
auto	wrap_180_cd (const T angle) -> decltype(wrap_180(angle, 100.f))
template auto	wrap_180_cd< float > (const float angle) -> decltype(wrap_180(angle, 100.f))
template auto	wrap_180_cd< int > (const int angle) -> decltype(wrap_180(angle, 100.f))
template auto	wrap_180_cd< long > (const long angle) -> decltype(wrap_180(angle, 100.f))
template auto	wrap_180_cd< short > (const short angle) -> decltype(wrap_180(angle, 100.f))
template auto	wrap_180_cd< double > (const double angle) -> decltype(wrap_360(angle, 100.f))
template<typename T >
float	wrap_360 (const T angle, float unit_mod)
template float	wrap_360< int > (const int angle, float unit_mod)
template float	wrap_360< short > (const short angle, float unit_mod)
template float	wrap_360< long > (const long angle, float unit_mod)
template float	wrap_360< float > (const float angle, float unit_mod)
template float	wrap_360< double > (const double angle, float unit_mod)
template<typename T >
auto	wrap_360_cd (const T angle) -> decltype(wrap_360(angle, 100.f))
template auto	wrap_360_cd< float > (const float angle) -> decltype(wrap_360(angle, 100.f))
template auto	wrap_360_cd< int > (const int angle) -> decltype(wrap_360(angle, 100.f))
template auto	wrap_360_cd< long > (const long angle) -> decltype(wrap_360(angle, 100.f))
template auto	wrap_360_cd< short > (const short angle) -> decltype(wrap_360(angle, 100.f))
template auto	wrap_360_cd< double > (const double angle) -> decltype(wrap_360(angle, 100.f))
template<typename T >
float	wrap_PI (const T radian)
template float	wrap_PI< int > (const int radian)
template float	wrap_PI< short > (const short radian)
template float	wrap_PI< float > (const float radian)
template float	wrap_PI< double > (const double radian)
template<typename T >
float	wrap_2PI (const T radian)
template float	wrap_2PI< int > (const int radian)
template float	wrap_2PI< short > (const short radian)
template float	wrap_2PI< float > (const float radian)
template float	wrap_2PI< double > (const double radian)
template<typename T >
T	constrain_value (const T amt, const T low, const T high)
template int	constrain_value< int > (const int amt, const int low, const int high)
template long	constrain_value< long > (const long amt, const long low, const long high)
template short	constrain_value< short > (const short amt, const short low, const short high)
template float	constrain_value< float > (const float amt, const float low, const float high)
template double	constrain_value< double > (const double amt, const double low, const double high)
uint16_t	get_random16 (void)
float	rand_float (void)
Vector3f	rand_vec3f (void)
bool	is_valid_octal (uint16_t octal)

Function Documentation

constrain_value()

template<typename T >

T constrain_value	(	const T	amt,
		const T	low,
		const T	high
	)

Definition at line 182 of file AP_Math.cpp.

Referenced by constrain_float(), constrain_int16(), constrain_int32(), is_negative(), and TEST().

Here is the call graph for this function:

Here is the caller graph for this function:

constrain_value< double >()

template double constrain_value< double >	(	const double	amt,
		const double	low,
		const double	high
	)

Referenced by constrain_value().

Here is the caller graph for this function:

constrain_value< float >()

template float constrain_value< float >	(	const float	amt,
		const float	low,
		const float	high
	)

Referenced by constrain_value().

Here is the caller graph for this function:

constrain_value< int >()

template int constrain_value< int >	(	const int	amt,
		const int	low,
		const int	high
	)

Referenced by constrain_value().

Here is the caller graph for this function:

constrain_value< long >()

template long constrain_value< long >	(	const long	amt,
		const long	low,
		const long	high
	)

Referenced by constrain_value().

Here is the caller graph for this function:

constrain_value< short >()

template short constrain_value< short >	(	const short	amt,
		const short	low,
		const short	high
	)

Referenced by constrain_value().

Here is the caller graph for this function:

get_random16()

uint16_t get_random16

(

void

)

Definition at line 212 of file AP_Math.cpp.

Referenced by rand_num(), FlashTest::setup(), CompassCalibrator::thin_samples(), and usec_to_hz().

Here is the caller graph for this function:

is_equal() [1/2]

template<typename Arithmetic1 , typename Arithmetic2 >

std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type is_equal	(	const Arithmetic1	v_1,
		const Arithmetic2	v_2
	)

Definition at line 12 of file AP_Math.cpp.

Referenced by AP_Baro_Backend::_copy_to_frontend(), AP_GeodesicGrid::_from_neighbor_umbrella(), GCS_MAVLINK::_handle_command_preflight_calibration(), HALSITL::SITL_State::_update_rangefinder(), AP_InertialSensor::accel_calibrated_ok_all(), NotchFilterVector3fParam::apply(), AP_TempCalibration::calculate_calibration(), AP_Camera::control(), AP_Param::convert_old_parameter(), AP_AHRS_DCM::drift_correction(), AP_AccelCal::gcs_vehicle_position(), GCS_MAVLINK::handle_command_long_message(), GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(), GCS_MAVLINK::handle_command_request_autopilot_capabilities(), GCS_MAVLINK::handle_param_set(), SoloGimbal_Parameters::handle_param_value(), GCS_MAVLINK::handle_preflight_reboot(), AC_Circle::init_start_angle(), Location_Class::offset(), Vector2< int32_t >::operator!=(), Vector3< int32_t >::operator!=(), Vector2< int32_t >::operator==(), Vector3< int32_t >::operator==(), AP_Param::save(), SoloGimbal_Parameters::set_param(), AC_Circle::set_rate(), Compass::setHIL(), setup(), TEST(), CompassCalibrator::update(), AP_L1_Control::update_waypoint(), and AP_AHRS_DCM::yaw_error_compass().

Here is the caller graph for this function:

is_equal() [2/2]

template<typename Arithmetic1 , typename Arithmetic2 >

std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type is_equal	(	const Arithmetic1	v_1,
		const Arithmetic2	v_2
	)

Definition at line 24 of file AP_Math.cpp.

Here is the call graph for this function:

is_equal< double >()

template bool is_equal< double >	(	const double	v_1,
		const double	v_2
	)

Referenced by is_equal().

Here is the caller graph for this function:

is_equal< float >()

template bool is_equal< float >	(	const float	v_1,
		const float	v_2
	)

Referenced by is_equal().

Here is the caller graph for this function:

is_equal< int >()

template bool is_equal< int >	(	const int	v_1,
		const int	v_2
	)

Referenced by is_equal().

Here is the caller graph for this function:

is_equal< long >()

template bool is_equal< long >	(	const long	v_1,
		const long	v_2
	)

Referenced by is_equal().

Here is the caller graph for this function:

is_equal< short >()

template bool is_equal< short >	(	const short	v_1,
		const short	v_2
	)

Referenced by is_equal().

Here is the caller graph for this function:

is_valid_octal()

bool is_valid_octal

(

uint16_t

octal

)

Definition at line 241 of file AP_Math.cpp.

Referenced by AP_ADSB::update(), and usec_to_hz().

Here is the caller graph for this function:

linear_interpolate()

float linear_interpolate	(	float	low_output,
		float	high_output,
		float	var_value,
		float	var_low,
		float	var_high
	)

Definition at line 81 of file AP_Math.cpp.

Referenced by AP_Tuning::check_input(), AP_Landing_Deepstall::override_servos(), and usec_to_hz().

Here is the caller graph for this function:

rand_float()

float rand_float

(

void

)

Definition at line 224 of file AP_Math.cpp.

Referenced by AP_Baro_SITL::_timer(), HALSITL::SITL_State::_update_airspeed(), HALSITL::SITL_State::_update_rangefinder(), AP_InertialSensor_SITL::generate_accel(), AP_InertialSensor_SITL::generate_gyro(), rand_vec3f(), and usec_to_hz().

Here is the caller graph for this function:

rand_vec3f()

Vector3f rand_vec3f

(

void

)

Definition at line 229 of file AP_Math.cpp.

Referenced by AP_Compass_SITL::_timer(), and usec_to_hz().

Here is the call graph for this function:

Here is the caller graph for this function:

safe_asin()

template<typename T >

float safe_asin

(

const T

v

)

Definition at line 43 of file AP_Math.cpp.

Referenced by Quaternion::get_euler_pitch(), is_negative(), AP_MotorsTri::output_armed_stabilizing(), Matrix3< float >::to_euler(), and AP_AHRS::update_AOA_SSA().

Here is the call graph for this function:

Here is the caller graph for this function:

safe_asin< double >()

template float safe_asin< double >

(

const double

v

)

Referenced by safe_asin().

Here is the caller graph for this function:

safe_asin< float >()

template float safe_asin< float >

(

const float

v

)

Referenced by safe_asin().

Here is the caller graph for this function:

safe_asin< int >()

template float safe_asin< int >

(

const int

v

)

Referenced by safe_asin().

Here is the caller graph for this function:

safe_asin< short >()

template float safe_asin< short >

(

const short

v

)

Referenced by safe_asin().

Here is the caller graph for this function:

safe_sqrt()

template<typename T >

float safe_sqrt

(

const T

v

)

Definition at line 64 of file AP_Math.cpp.

Referenced by AP_InertialSensor::_acal_save_calibrations(), AP_GPS_SBP::_attempt_state_update(), AP_GPS_SBP2::_attempt_state_update(), AC_WPNav::advance_spline_target_along_track(), AC_WPNav::advance_wp_target_along_track(), AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(), AP_AHRS::calc_trig(), AC_Circle::calc_velocities(), Vector2< int32_t >::circle_segment_intersection(), Vector3< int32_t >::distance_to_segment(), AP_Baro::get_EAS2TAS(), AC_Avoid::get_max_speed(), AC_WPNav::get_slow_down_speed(), AP_InertialSensor::get_vibration_levels(), is_negative(), DataFlash_Class::Log_Write_POS(), AP_GPS_SBF::process_message(), AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(), AC_PosControl::set_alt_target_from_climb_rate_ff(), AC_WPNav::set_spline_origin_and_destination(), AC_WPNav::set_wp_origin_and_destination(), AC_AttitudeControl::sqrt_controller(), and AC_PosControl::sqrt_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

safe_sqrt< double >()

template float safe_sqrt< double >

(

const double

v

)

Referenced by safe_sqrt().

Here is the caller graph for this function:

safe_sqrt< float >()

template float safe_sqrt< float >

(

const float

v

)

Referenced by safe_sqrt().

Here is the caller graph for this function:

safe_sqrt< int >()

template float safe_sqrt< int >

(

const int

v

)

Referenced by safe_sqrt().

Here is the caller graph for this function:

safe_sqrt< short >()

template float safe_sqrt< short >

(

const short

v

)

Referenced by safe_sqrt().

Here is the caller graph for this function:

wrap_180()

template<typename T >

float wrap_180	(	const T	angle,
		float	unit_mod
	)

Definition at line 96 of file AP_Math.cpp.

Referenced by AP_Proximity_Backend::convert_angle_to_sector(), AP_Beacon_Backend::correct_for_orient_yaw(), is_negative(), AP_Landing_Deepstall::verify_land(), and wrap_180_cd().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_180< double >()

template float wrap_180< double >	(	const double	angle,
		float	unit_mod
	)

Referenced by wrap_180().

Here is the caller graph for this function:

wrap_180< float >()

template float wrap_180< float >	(	const float	angle,
		float	unit_mod
	)

Referenced by wrap_180().

Here is the caller graph for this function:

wrap_180< int >()

template float wrap_180< int >	(	const int	angle,
		float	unit_mod
	)

Referenced by wrap_180().

Here is the caller graph for this function:

wrap_180< short >()

template float wrap_180< short >	(	const short	angle,
		float	unit_mod
	)

Referenced by wrap_180().

Here is the caller graph for this function:

wrap_180_cd()

template<typename T >

auto wrap_180_cd

(

const T

angle

)

-> decltype(wrap_180(angle, 100.f))

Definition at line 111 of file AP_Math.cpp.

Referenced by AP_L1_Control::_prevent_indecision(), AR_AttitudeControl::get_forward_speed(), AP_L1_Control::get_yaw_sensor(), AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(), AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(), is_negative(), AP_L1_Control::nav_bearing_cd(), AP_L1_Control::target_bearing_cd(), TEST(), test_wrap_cd(), AP_L1_Control::update_heading_hold(), AP_AHRS_DCM::use_compass(), and AP_Landing_Deepstall::verify_land().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_180_cd< double >()

template auto wrap_180_cd< double >

(

const double

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_180_cd().

Here is the caller graph for this function:

wrap_180_cd< float >()

template auto wrap_180_cd< float >

(

const float

angle

)

-> decltype(wrap_180(angle, 100.f))

Referenced by wrap_180_cd().

Here is the caller graph for this function:

wrap_180_cd< int >()

template auto wrap_180_cd< int >

(

const int

angle

)

-> decltype(wrap_180(angle, 100.f))

Referenced by wrap_180_cd().

Here is the caller graph for this function:

wrap_180_cd< long >()

template auto wrap_180_cd< long >

(

const long

angle

)

-> decltype(wrap_180(angle, 100.f))

Referenced by wrap_180_cd().

Here is the caller graph for this function:

wrap_180_cd< short >()

template auto wrap_180_cd< short >

(

const short

angle

)

-> decltype(wrap_180(angle, 100.f))

Referenced by wrap_180_cd().

Here is the caller graph for this function:

wrap_2PI()

template<typename T >

float wrap_2PI

(

const T

radian

)

Definition at line 167 of file AP_Math.cpp.

Referenced by AP_Beacon::get_next_boundary_point(), AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(), is_negative(), AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(), CompassLearn::process_sample(), TEST(), and wrap_PI().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_2PI< double >()

template float wrap_2PI< double >

(

const double

radian

)

Referenced by wrap_2PI().

Here is the caller graph for this function:

wrap_2PI< float >()

template float wrap_2PI< float >

(

const float

radian

)

Referenced by wrap_2PI().

Here is the caller graph for this function:

wrap_2PI< int >()

template float wrap_2PI< int >

(

const int

radian

)

Referenced by wrap_2PI().

Here is the caller graph for this function:

wrap_2PI< short >()

template float wrap_2PI< short >

(

const short

radian

)

Referenced by wrap_2PI().

Here is the caller graph for this function:

wrap_360()

template<typename T >

float wrap_360	(	const T	angle,
		float	unit_mod
	)

Definition at line 123 of file AP_Math.cpp.

Referenced by AP_GPS_SBP::_attempt_state_update(), AP_GPS_SBP2::_attempt_state_update(), AP_GPS_SIRF::_parse_gps(), AP_GPS_ERB::_parse_gps(), AP_GPS_UBLOX::_parse_gps(), AP_GPS_NMEA::_term_complete(), AP_GPS::calc_blended_state(), AP_Proximity_SITL::get_distance_to_fence(), AP_Proximity_Backend::get_next_ignore_start_or_end(), AP_GPS_MAV::handle_msg(), AP_Proximity_LightWareSF40C::init_sectors(), AP_Proximity_RPLidarA2::init_sectors(), is_negative(), AP_GPS_SBF::process_message(), CompassLearn::process_sample(), AP_GPS_MTK::read(), AP_GPS_MTK19::read(), AP_GPS::setHIL(), wrap_180(), wrap_180_cd(), and wrap_360_cd().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_360< double >()

template float wrap_360< double >	(	const double	angle,
		float	unit_mod
	)

Referenced by wrap_360().

Here is the caller graph for this function:

wrap_360< float >()

template float wrap_360< float >	(	const float	angle,
		float	unit_mod
	)

Referenced by wrap_360().

Here is the caller graph for this function:

wrap_360< int >()

template float wrap_360< int >	(	const int	angle,
		float	unit_mod
	)

Referenced by wrap_360().

Here is the caller graph for this function:

wrap_360< long >()

template float wrap_360< long >	(	const long	angle,
		float	unit_mod
	)

Referenced by wrap_360().

Here is the caller graph for this function:

wrap_360< short >()

template float wrap_360< short >	(	const short	angle,
		float	unit_mod
	)

Referenced by wrap_360().

Here is the caller graph for this function:

wrap_360_cd()

template<typename T >

auto wrap_360_cd

(

const T

angle

)

-> decltype(wrap_360(angle, 100.f))

Definition at line 140 of file AP_Math.cpp.

Referenced by is_negative(), DataFlash_Class::Log_Write_AHRS2(), DataFlash_Class::Log_Write_POS(), SITL::SITL::Log_Write_SIMSTATE(), SITL::ADSB::send_report(), TEST(), and test_wrap_cd().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_360_cd< double >()

template auto wrap_360_cd< double >

(

const double

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_360_cd().

Here is the caller graph for this function:

wrap_360_cd< float >()

template auto wrap_360_cd< float >

(

const float

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_360_cd().

Here is the caller graph for this function:

wrap_360_cd< int >()

template auto wrap_360_cd< int >

(

const int

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_360_cd().

Here is the caller graph for this function:

wrap_360_cd< long >()

template auto wrap_360_cd< long >

(

const long

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_360_cd().

Here is the caller graph for this function:

wrap_360_cd< short >()

template auto wrap_360_cd< short >

(

const short

angle

)

-> decltype(wrap_360(angle, 100.f))

Referenced by wrap_360_cd().

Here is the caller graph for this function:

wrap_PI()

template<typename T >

float wrap_PI

(

const T

radian

)

Definition at line 152 of file AP_Math.cpp.

Referenced by AP_Mount_Backend::calc_angle_to_location(), NavEKF2_core::calcQuatAndFieldStates(), NavEKF3_core::calcQuatAndFieldStates(), AP_AHRS_DCM::drift_correction_yaw(), NavEKF2_core::fuseEulerYaw(), NavEKF3_core::fuseEulerYaw(), AR_AttitudeControl::get_steering_out_heading(), AP_L1_Control::get_yaw(), NavEKF2::getLastYawResetAngle(), NavEKF3::getLastYawResetAngle(), AC_Circle::init_start_angle(), AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(), AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(), AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(), AC_AttitudeControl::input_quaternion(), AC_AttitudeControl::input_shaping_rate_predictor(), is_negative(), AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(), NavEKF2_core::realignYawGPS(), NavEKF3_core::realignYawGPS(), AC_Loiter::set_pilot_desired_acceleration(), TEST(), test_wrap_cd(), AC_AttitudeControl::thrust_heading_rotation_angles(), Quaternion::to_axis_angle(), AC_Circle::update(), AP_Landing_Deepstall::update_steering(), NavEKF2::updateLaneSwitchYawResetData(), and NavEKF3::updateLaneSwitchYawResetData().

Here is the call graph for this function:

Here is the caller graph for this function:

wrap_PI< double >()

template float wrap_PI< double >

(

const double

radian

)

Referenced by wrap_PI().

Here is the caller graph for this function:

wrap_PI< float >()

template float wrap_PI< float >

(

const float

radian

)

Referenced by wrap_PI().

Here is the caller graph for this function:

wrap_PI< int >()

template float wrap_PI< int >

(

const int

radian

)

Referenced by wrap_PI().

Here is the caller graph for this function:

wrap_PI< short >()

template float wrap_PI< short >

(

const short

radian

)

Referenced by wrap_PI().

Here is the caller graph for this function: