APM:Libraries
|
#include <vector3.h>
Public Member Functions | |
constexpr | Vector3 () |
constexpr | Vector3 (const T x0, const T y0, const T z0) |
void | operator() (const T x0, const T y0, const T z0) |
bool | operator== (const Vector3< T > &v) const |
bool | operator!= (const Vector3< T > &v) const |
Vector3< T > | operator- (void) const |
Vector3< T > | operator+ (const Vector3< T > &v) const |
Vector3< T > | operator- (const Vector3< T > &v) const |
Vector3< T > | operator* (const T num) const |
Vector3< T > | operator/ (const T num) const |
Vector3< T > & | operator+= (const Vector3< T > &v) |
Vector3< T > & | operator-= (const Vector3< T > &v) |
Vector3< T > & | operator*= (const T num) |
Vector3< T > & | operator/= (const T num) |
T & | operator[] (uint8_t i) |
const T & | operator[] (uint8_t i) const |
T | operator* (const Vector3< T > &v) const |
Vector3< T > | operator* (const Matrix3< T > &m) const |
Matrix3< T > | mul_rowcol (const Vector3< T > &v) const |
Vector3< T > | operator% (const Vector3< T > &v) const |
float | angle (const Vector3< T > &v2) const |
bool | is_nan (void) const |
bool | is_inf (void) const |
bool | is_zero (void) const |
void | rotate (enum Rotation rotation) |
void | rotate_inverse (enum Rotation rotation) |
T | length_squared () const |
float | length (void) const |
void | normalize () |
void | zero () |
Vector3< T > | normalized () const |
void | reflect (const Vector3< T > &n) |
void | project (const Vector3< T > &v) |
Vector3< T > | projected (const Vector3< T > &v) const |
float | distance_squared (const Vector3< T > &v) const |
float | distance_to_segment (const Vector3< T > &seg_start, const Vector3< T > &seg_end) const |
Static Public Member Functions | |
static Vector3< T > | perpendicular (const Vector3< T > &p1, const Vector3< T > &v1) |
Public Attributes | |
T | x |
T | y |
T | z |
Definition at line 376 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator[]().
Definition at line 214 of file vector3.h.
Referenced by AP_SmartRTL::add_loop(), and AP_SmartRTL::add_point().
float Vector3< T >::distance_to_segment | ( | const Vector3< T > & | seg_start, |
const Vector3< T > & | seg_end | ||
) | const |
Definition at line 410 of file vector3.cpp.
Referenced by AP_SmartRTL::detect_simplifications(), Vector3< int32_t >::distance_squared(), and Vector3< int32_t >::distance_to_segment().
template bool Vector3< T >::is_inf | ( | void | ) | const |
Definition at line 321 of file vector3.cpp.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), Vector3< int32_t >::distance_to_segment(), AP_AHRS_DCM::drift_correction(), AP_Compass_Backend::field_ok(), and Vector3< int32_t >::operator[]().
template bool Vector3< T >::is_nan | ( | void | ) | const |
Definition at line 315 of file vector3.cpp.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), Vector3< int32_t >::distance_to_segment(), AP_Compass_Backend::field_ok(), NavEKF2_core::getFilterFaults(), NavEKF3_core::getFilterFaults(), Matrix3< float >::is_nan(), and Vector3< int32_t >::operator[]().
|
inline |
Definition at line 159 of file vector3.h.
Referenced by HALSITL::SITL_State::_update_gps(), HALSITL::SITL_State::_update_rangefinder(), SoloGimbalEKF::calcMagHeadingInnov(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), Compass::consistent(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), AP_InertialSensor_SITL::generate_accel(), AP_Follow::get_offsets_ned(), AP_Avoidance::get_vector_perpendicular(), AP_AHRS_DCM::ra_delayed(), NavEKF2_core::selectHeightForFusion(), NavEKF3_core::selectHeightForFusion(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), and SITL::FlightAxis::update().
template float Vector3< T >::length | ( | void | ) | const |
Definition at line 288 of file vector3.cpp.
Referenced by AP_InertialSensor::_acal_save_calibrations(), AP_InertialSensor::_init_gyro(), AC_WPNav::advance_spline_target_along_track(), AC_WPNav::advance_wp_target_along_track(), AP_GPS::all_consistent(), Vector3< int32_t >::angle(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), SITL::Frame::calculate_forces(), AP_Frsky_Telem::check_ekf_status(), NavEKF2_core::checkAttitudeAlignmentStatus(), AP_Arming::compass_checks(), NavEKF2_core::controlMagYawReset(), NavEKF3_core::controlMagYawReset(), NavEKF2_core::detectOptFlowTakeoff(), NavEKF3_core::detectOptFlowTakeoff(), Vector3< int32_t >::distance_to_segment(), AP_AHRS_DCM::drift_correction(), AP_AHRS_DCM::drift_correction_yaw(), AP_AHRS_DCM::estimate_wind(), AP_Compass_Backend::field_ok(), Quaternion::from_axis_angle(), Quaternion::from_axis_angle_fast(), NavEKF2_core::FuseMagnetometer(), NavEKF3_core::FuseMagnetometer(), NavEKF2_core::FuseRngBcn(), NavEKF3_core::FuseRngBcn(), AP_Follow::get_target_dist_and_vel_ned(), IRLock::get_unit_vector_body(), AP_Avoidance::get_vector_perpendicular(), AC_PrecLand_Companion::handle_msg(), AP_Landing::head_wind(), AC_WPNav::init_brake_target(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), AP_Arming::ins_checks(), Vector3< int32_t >::length_squared(), Vector3< int32_t >::normalize(), Matrix3< float >::normalize(), Vector3< int32_t >::normalized(), AP_Beacon_Pozyx::parse_buffer(), rand_vec3f(), AP_AHRS_DCM::renorm(), AP_AHRS_DCM::reset(), Quaternion::rotate_fast(), AC_PrecLand::run_estimator(), run_test(), AP_SmartRTL::segment_segment_dist(), NavEKF2_core::send_status_report(), NavEKF3_core::send_status_report(), AC_WPNav::set_spline_origin_and_destination(), AC_WPNav::set_wp_origin_and_destination(), AP_InertialSensor::simple_accel_cal(), SITL::Aircraft::smooth_sensors(), test_euler(), test_matrix_rotate(), AC_AttitudeControl::thrust_heading_rotation_angles(), CompassLearn::update(), AP_Beacon_SITL::update(), AC_PrecLand_SITL::update(), AP_AHRS::update_AOA_SSA(), SITL::Aircraft::update_dynamics(), NavEKF2_core::UpdateStrapdownEquationsNED(), NavEKF3_core::UpdateStrapdownEquationsNED(), and AP_AHRS_DCM::use_compass().
|
inline |
Definition at line 167 of file vector3.h.
Referenced by Vector3< int32_t >::perpendicular().
template Matrix3< double > Vector3< T >::mul_rowcol | ( | const Vector3< T > & | v | ) | const |
Definition at line 400 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), Vector3< int32_t >::operator[](), and Airspeed_Calibration::update().
|
inline |
Definition at line 176 of file vector3.h.
Referenced by AP_AHRS_DCM::drift_correction(), AP_Avoidance::get_vector_perpendicular(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(), rand_vec3f(), and NavEKF3_core::SelectBodyOdomFusion().
Definition at line 188 of file vector3.h.
Referenced by Compass::consistent(), and Matrix3< float >::from_axis_angle().
Definition at line 370 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
template Vector3< double > Vector3< T >::operator% | ( | const Vector3< T > & | v | ) | const |
Definition at line 274 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator[]().
|
inline |
Definition at line 340 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), Vector3< int32_t >::operator()(), and Vector3< int32_t >::operator[]().
Definition at line 282 of file vector3.cpp.
Definition at line 391 of file vector3.cpp.
Definition at line 294 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
template Vector3< double > Vector3< T >::operator+ | ( | const Vector3< T > & | v | ) | const |
Definition at line 352 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
template Vector3< double > & Vector3< T >::operator+= | ( | const Vector3< T > & | v | ) |
Definition at line 327 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
Definition at line 358 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
Definition at line 346 of file vector3.cpp.
template Vector3< double > & Vector3< T >::operator-= | ( | const Vector3< T > & | v | ) |
Definition at line 308 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
Definition at line 334 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
Definition at line 301 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
Definition at line 364 of file vector3.cpp.
Referenced by Vector3< int32_t >::distance_to_segment(), and Vector3< int32_t >::operator()().
|
inline |
|
inline |
|
inlinestatic |
Definition at line 228 of file vector3.h.
Referenced by Vector3< int32_t >::perpendicular(), and AP_Avoidance::perpendicular_xyz().
Definition at line 202 of file vector3.h.
Referenced by Vector3< int32_t >::reflect().
Definition at line 28 of file vector3.cpp.
Referenced by AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_InertialSensor_Backend::_rotate_and_correct_gyro(), Vector3< int32_t >::distance_to_segment(), AP_InertialSensor::get_fixed_mount_accel_cal_sample(), AP_AHRS_View::get_gyro_latest(), AP_InertialSensor::get_primary_accel_cal_sample_avg(), have_rotation(), Vector3< int32_t >::is_zero(), AP_Compass_Backend::rotate_field(), Vector3< int32_t >::rotate_inverse(), AP_VisualOdom_Backend::set_deltas(), test_euler(), test_rotate_matrix(), AP_Compass_QMC5883L::timer(), and AP_AHRS_View::update().
Definition at line 253 of file vector3.cpp.
Referenced by AP_InertialSensor_Backend::_publish_accel(), Vector3< int32_t >::distance_to_segment(), Vector3< int32_t >::is_zero(), and AP_InertialSensor::simple_accel_cal().
|
inline |
Definition at line 182 of file vector3.h.
Referenced by AP_InertialSensor::_init_gyro(), AP_Mount_Backend::calc_angle_to_location(), AP_GPS::calc_blended_state(), CompassCalibrator::calc_initial_offset(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), Compass_PerMotor::calibration_start(), Compass_PerMotor::compensate(), AP_Compass_Backend::correct_field(), AP_AHRS_DCM::drift_correction(), AP_AHRS_DCM::drift_correction_yaw(), NavEKF2_core::FuseAirspeed(), SoloGimbalEKF::fuseCompass(), NavEKF2_core::FuseDeclination(), NavEKF2_core::fuseEulerYaw(), NavEKF2_core::FuseMagnetometer(), NavEKF2_core::FuseOptFlow(), NavEKF2_core::FuseRngBcn(), NavEKF2_core::FuseRngBcnStatic(), NavEKF3_core::FuseRngBcnStatic(), NavEKF2_core::FuseSideslip(), SoloGimbalEKF::fuseVelocity(), NavEKF2_core::FuseVelPosNED(), AP_AHRS_NavEKF::get_variances(), NavEKF3_core::getAccelBias(), AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(), AP_AHRS::getCorrectedDeltaVelocityNED(), SoloGimbalEKF::getDebug(), SoloGimbalEKF::getGyroBias(), NavEKF2_core::getGyroBias(), NavEKF3_core::getGyroBias(), AP_AHRS_NavEKF::getMagOffsets(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AP_AHRS_DCM::matrix_update(), AP_Compass_SITL::read(), AP_Compass_BMM150::read(), AP_Compass_MMC3416::read(), AP_Compass_IST8310::read(), AP_Compass_QMC5883L::read(), AP_Compass_LIS3MDL::read(), AP_Compass_AK09916::read(), NavEKF2_core::readIMUData(), NavEKF3_core::readIMUData(), NavEKF2_core::readMagData(), NavEKF3_core::readMagData(), AP_AHRS_DCM::reset(), AP_AHRS_DCM::reset_gyro_drift(), CompassCalibrator::reset_state(), NavEKF2_core::resetGyroBias(), NavEKF3_core::resetGyroBias(), NavEKF2_core::ResetVelocity(), NavEKF3_core::ResetVelocity(), NavEKF2_core::SelectMagFusion(), NavEKF3_core::SelectMagFusion(), NavEKF3_core::SelectVelPosFusion(), GCS_MAVLINK::send_raw_imu(), SITL::Gimbal::send_report(), AP_InertialSensor::simple_accel_cal(), SITL::Tracker::update(), SITL::FlightAxis::update(), AC_Sprayer::update(), AP_InertialSensor::update(), SITL::Aircraft::update_dynamics(), AP_AHRS_NavEKF::update_EKF2(), AP_AHRS_NavEKF::update_EKF3(), AP_AHRS_NavEKF::update_SITL(), AP_AHRS_NavEKF::wind_estimate(), NavEKF2_core::writeOptFlowMeas(), and NavEKF3_core::writeOptFlowMeas().
T Vector3< T >::x |
Definition at line 67 of file vector3.h.
Referenced by AP_InertialSensor::_acal_save_calibrations(), AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(), AP_InertialSensor::_calculate_trim(), AP_GeodesicGrid::_from_neighbor_umbrella(), AP_RollController::_get_rate_out(), AP_Compass_AK8963::_make_factory_sensitivity_adjustment(), AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_GPS_ERB::_parse_gps(), AP_GPS_UBLOX::_parse_gps(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_Compass_MAG3110::_update(), AP_Compass_LSM303D::_update(), HALSITL::SITL_State::_update_gps(), AP_InertialSensor::accel_calibrated_ok_all(), AccelCalibrator::accept_result(), AP_Compass_MMC3416::accumulate_field(), AP_AHRS::add_trim(), AC_Avoid::adjust_velocity(), AC_WPNav::advance_spline_target_along_track(), AC_WPNav::advance_wp_target_along_track(), NavEKF2_core::alignMagStateDeclination(), NavEKF3_core::alignMagStateDeclination(), AC_AttitudeControl::ang_vel_limit(), AC_AttitudeControl::ang_vel_to_euler_rate(), AP_AHRS::AP_AHRS(), AC_AttitudeControl::attitude_controller_run_quat(), AP_Landing_Deepstall::build_approach_path(), AP_GPS::calc_blended_state(), AC_Loiter::calc_desired_velocity(), CompassCalibrator::calc_ellipsoid_jacob(), AccelCalibrator::calc_jacob(), CompassCalibrator::calc_residual(), AccelCalibrator::calc_residual(), CompassCalibrator::calc_sphere_jacob(), AP_AHRS::calc_trig(), AP_InertialSensor::calc_vibration_and_clipping(), NavEKF2_core::calcEarthRateNED(), NavEKF3_core::calcEarthRateNED(), NavEKF2_core::calcGpsGoodToAlign(), NavEKF3_core::calcGpsGoodToAlign(), SoloGimbalEKF::calcMagHeadingInnov(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), NavEKF2_core::calcQuatAndFieldStates(), NavEKF3_core::calcQuatAndFieldStates(), NavEKF2_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::calcRotVecVariances(), SITL::Frame::calculate_forces(), SITL::Submarine::calculate_forces(), SITL::Plane::calculate_forces(), Compass::calculate_heading(), AC_WPNav::calculate_wp_leash_length(), AP_AHRS_DCM::check_matrix(), check_path(), NavEKF3_core::checkAttitudeAlignmentStatus(), Matrix3< float >::colx(), Compass::consistent(), AC_PrecLand::construct_pos_meas_using_rangefinder(), AP_Mount_Alexmos::control_axis(), NavEKF2_core::controlMagYawReset(), NavEKF3_core::controlMagYawReset(), SITL::SITL::convert_earth_frame(), AP_Compass_Backend::correct_field(), AP_Beacon_Backend::correct_for_orient_yaw(), NavEKF2_core::correctDeltaAngle(), NavEKF2_core::correctEkfOriginHeight(), NavEKF3_core::correctEkfOriginHeight(), NavEKF3_core::CovarianceInit(), NavEKF2_core::CovariancePrediction(), AC_PosControl::desired_accel_to_vel(), AC_PosControl::desired_vel_to_pos(), NavEKF2_core::detectFlight(), NavEKF3_core::detectFlight(), display_offsets_and_scaling(), Vector3< int32_t >::distance_squared(), AP_AHRS_DCM::drift_correction(), AP_AHRS_DCM::estimate_wind(), NavEKF2_core::EstimateTerrainOffset(), NavEKF3_core::EstimateTerrainOffset(), AC_AttitudeControl::euler_accel_limit(), AC_AttitudeControl::euler_rate_to_ang_vel(), AP_GPS_Backend::fill_3d_velocity(), SITL::Aircraft::fill_fdm(), CompassCalibrator::fit_acceptable(), Quaternion::from_axis_angle(), Matrix3< float >::from_axis_angle(), Quaternion::from_axis_angle_fast(), Quaternion::from_rotation_matrix(), NavEKF2_core::FuseAirspeed(), NavEKF3_core::FuseAirspeed(), NavEKF3_core::FuseBodyVel(), SoloGimbalEKF::fuseCompass(), NavEKF2_core::FuseDeclination(), NavEKF3_core::FuseDeclination(), NavEKF2_core::fuseEulerYaw(), NavEKF3_core::fuseEulerYaw(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), NavEKF2_core::FuseRngBcn(), NavEKF3_core::FuseRngBcn(), NavEKF2_core::FuseRngBcnStatic(), NavEKF3_core::FuseRngBcnStatic(), NavEKF2_core::FuseSideslip(), NavEKF3_core::FuseSideslip(), SoloGimbalEKF::fuseVelocity(), NavEKF2_core::FuseVelPosNED(), NavEKF3_core::FuseVelPosNED(), AP_InertialSensor_SITL::generate_accel(), AP_InertialSensor_SITL::generate_gyro(), SoloGimbal_Parameters::get_accel_bias(), SoloGimbal_Parameters::get_accel_gain(), get_bearing_cd(), AC_Circle::get_closest_point_on_circle(), CompassCalibrator::param_t::get_ellipsoid_params(), AR_AttitudeControl::get_forward_speed(), SoloGimbal_Parameters::get_gyro_bias(), get_horizontal_distance_cm(), AC_PosControl::get_horizontal_error(), SoloGimbal_Parameters::get_joint_bias(), AP_Follow::get_offsets_ned(), AP_AHRS_NavEKF::get_relative_position_NED_home(), AP_AHRS_NavEKF::get_relative_position_NED_origin(), AccelCalibrator::get_sample_corrected(), AC_PosControl::get_stopping_point_xy(), AP_Follow::get_target_location_and_velocity(), IRLock::get_unit_vector_body(), Location_Class::get_vector_from_origin_NEU(), AC_WPNav::get_vector_NEU(), AP_InertialSensor::get_vibration_levels(), SoaringController::get_wind_corrected_drift(), AC_WPNav::get_wp_destination(), AC_WPNav::get_wp_distance_to_destination(), NavEKF3_core::getBodyFrameOdomDebug(), SoloGimbalEKF::getDebug(), NavEKF2_core::getEulerAngles(), NavEKF3_core::getEulerAngles(), SITL::Plane::getForce(), NavEKF2_core::getGyroScaleErrorPercentage(), NavEKF2_core::getInnovations(), NavEKF3_core::getInnovations(), NavEKF2_core::getLLH(), NavEKF3_core::getLLH(), NavEKF2_core::getPosNE(), NavEKF3_core::getPosNE(), SITL::Plane::getTorque(), NavEKF2_core::getVariances(), NavEKF3_core::getVariances(), NavEKF2_core::getWind(), NavEKF3_core::getWind(), AP_AHRS_NavEKF::groundspeed_vector(), AP_AHRS::groundspeed_vector(), GCS_MAVLINK::handle_att_pos_mocap(), AP_GPS_MAV::handle_msg(), AP_Follow::handle_msg(), Matrix3< float >::identity(), AC_AttitudeControl::inertial_frame_reset(), AC_Circle::init(), AC_WPNav::init_brake_target(), AC_Circle::init_start_angle(), AC_Loiter::init_target(), AC_PosControl::init_vel_controller_xyz(), AC_PosControl::init_xy_controller(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), NavEKF3_core::initialiseQuatCovariances(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(), 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_rate_bf_roll_pitch_yaw(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(), AC_AttitudeControl::input_shaping_rate_predictor(), AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors(), AP_InertialSensor::is_still(), Location_Class::Location_Class(), AP_InertialSensor_Backend::log_accel_raw(), AP_InertialSensor_Backend::log_gyro_raw(), AP_Airspeed::log_mavlink_send(), DataFlash_Class::Log_Write_AHRS2(), DataFlash_Class::Log_Write_Attitude(), DataFlash_Class::Log_Write_AttitudeView(), DataFlash_Class::Log_Write_Beacon(), DataFlash_Class::Log_Write_Compass_instance(), DataFlash_Class::Log_Write_IMU_instance(), DataFlash_Class::Log_Write_IMUDT_instance(), DataFlash_Class::Log_Write_POS(), DataFlash_Class::Log_Write_Rate(), DataFlash_Class::Log_Write_SRTL(), DataFlash_Class::Log_Write_Vibration(), DataFlash_Class::Log_Write_VisualOdom(), loop(), Vector3< int32_t >::mul_rowcol(), Matrix3< float >::mul_transpose(), Matrix3< float >::mulXY(), Vector3< int32_t >::operator!=(), Vector3< int32_t >::operator%(), Vector3< int32_t >::operator*(), Matrix3< float >::operator*(), Vector3< int32_t >::operator+(), Vector3< int32_t >::operator+=(), Vector3< int32_t >::operator-(), Vector3< int32_t >::operator-=(), Vector3< int32_t >::operator==(), Vector3< int32_t >::operator[](), AP_Mount_Alexmos::parse_body(), AP_Mount_SToRM32_serial::parse_reply(), AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(), AP_Landing_Deepstall::predict_travel_distance(), SoloGimbalEKF::predictCovariance(), SoloGimbalEKF::predictStates(), print_vector(), AP_GPS_SBF::process_message(), CompassLearn::process_sample(), AC_AttitudeControl::rate_bf_roll_target(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), AC_AttitudeControl_Sub::rate_controller_run(), AC_AttitudeControl_Heli::rate_controller_run(), AC_AttitudeControl_Multi::rate_controller_run(), NavEKF2_core::readRngBcnData(), NavEKF3_core::readRngBcnData(), NavEKF2_core::realignYawGPS(), NavEKF3_core::realignYawGPS(), SITL::XPlane::receive_data(), SITL::CRRCSim::recv_fdm(), SITL::FlightAxis::report_FPS(), AP_AHRS_DCM::reset(), NavEKF2_core::ResetPosition(), NavEKF3_core::ResetPosition(), NavEKF2_core::ResetVelocity(), NavEKF3_core::ResetVelocity(), Matrix3< float >::rotate(), Quaternion::rotate_fast(), Vector3< int32_t >::rotate_inverse(), Quaternion::rotation_matrix(), Quaternion::rotation_matrix_norm(), AC_PrecLand::run_estimator(), AC_PrecLand::run_output_prediction(), run_test(), AC_PosControl::run_xy_controller(), SoloGimbalEKF::RunEKF(), AP_InertialSensor::BatchSampler::sample(), NavEKF3_core::SelectBodyOdomFusion(), NavEKF2_core::selectHeightForFusion(), NavEKF3_core::selectHeightForFusion(), NavEKF2_core::SelectMagFusion(), NavEKF3_core::SelectMagFusion(), NavEKF3_core::SelectRngBcnFusion(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), GCS_MAVLINK::send_ahrs(), GCS_MAVLINK::send_ahrs2(), GCS_MAVLINK::send_attitude(), AP_ADSB::send_dynamic_out(), GCS_MAVLINK::send_global_position_int(), GCS_MAVLINK::send_local_position(), Compass::send_mag_cal_progress(), Compass::send_mag_cal_report(), AP_Param::send_parameter(), GCS_MAVLINK::send_raw_imu(), SITL::ADSB::send_report(), SITL::Gimbal::send_report(), GCS_MAVLINK::send_sensor_offsets(), GCS_MAVLINK::send_vibration(), CompassCalibrator::CompassSample::set(), SoloGimbal_Parameters::set_accel_bias(), SoloGimbal_Parameters::set_accel_gain(), AP_InertialSensor::set_accel_peak_hold(), AP_Mount_Backend::set_angle_targets(), AC_PosControl::set_desired_accel_xy(), AC_PosControl::set_desired_velocity_xy(), SoloGimbal_Parameters::set_gyro_bias(), AC_PID_2D::set_input(), AC_PI_2D::set_input(), AC_PID_2D::set_integrator(), AC_PI_2D::set_integrator(), AC_WPNav::set_spline_origin_and_destination(), AP_AHRS::set_trim(), AC_WPNav::set_wp_destination_NED(), AC_WPNav::set_wp_origin_and_destination(), AC_PosControl::set_xy_target(), NavEKF2_core::setAidingMode(), NavEKF3_core::setAidingMode(), AP_GPS::setHIL(), NavEKF2_core::setWindMagStateLearningMode(), NavEKF3_core::setWindMagStateLearningMode(), AC_PosControl::shift_pos_xy_target(), SITL::Aircraft::smooth_sensors(), AC_Loiter::soften_for_landing(), AC_PosControl::sqrt_controller(), AP_Mount_Servo::stabilize(), AP_Mount_SToRM32_serial::status_msg(), AP_Mount_SToRM32::status_msg(), AP_Mount_Servo::status_msg(), AP_Mount_Alexmos::status_msg(), test_euler(), test_frame_transforms(), test_matrix_rotate(), AC_AttitudeControl::thrust_heading_rotation_angles(), AP_Compass_QMC5883L::timer(), AP_Compass_MMC3416::timer(), AP_OpticalFlow_Pixart::timer(), CompassLearn::update(), AP_OpticalFlow_SITL::update(), Airspeed_Calibration::update(), AP_Mount_SToRM32_serial::update(), SITL::Gimbal::update(), AP_Mount_SToRM32::update(), SITL::SimRover::update(), SITL::Helicopter::update(), SITL::SingleCopter::update(), SITL::FlightAxis::update(), AP_Mount_Servo::update(), AC_Circle::update(), AC_Sprayer::update(), AP_AHRS_NavEKF::update(), AP_TECS::update_50hz(), AC_AttitudeControl::update_ang_vel_target_from_att_error(), AP_AHRS::update_AOA_SSA(), AP_Beacon::update_boundary_points(), CompassCalibrator::update_completion_mask(), AP_AHRS_NavEKF::update_DCM(), SITL::Aircraft::update_dynamics(), AP_AHRS_NavEKF::update_EKF2(), AP_AHRS_NavEKF::update_EKF3(), SITL::Aircraft::update_position(), AP_Mount_Backend::update_targets_from_rc(), AP_AHRS::update_trig(), SITL::Vicon::update_vicon_position_estimate(), NavEKF2_core::UpdateStrapdownEquationsNED(), NavEKF3_core::UpdateStrapdownEquationsNED(), AP_Landing::wind_alignment(), AC_PosControl::write_log(), NavEKF2_core::writeOptFlowMeas(), and NavEKF3_core::writeOptFlowMeas().
T Vector3< T >::y |
Definition at line 67 of file vector3.h.
Referenced by AP_InertialSensor::_acal_save_calibrations(), AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(), AP_InertialSensor::_calculate_trim(), AP_GeodesicGrid::_from_neighbor_umbrella(), AP_PitchController::_get_rate_out(), AP_Compass_AK8963::_make_factory_sensitivity_adjustment(), AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_GPS_ERB::_parse_gps(), AP_GPS_UBLOX::_parse_gps(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_Compass_MAG3110::_update(), AP_Compass_LSM303D::_update(), HALSITL::SITL_State::_update_gps(), AP_TECS::_update_throttle_with_airspeed(), AP_TECS::_update_throttle_without_airspeed(), AP_InertialSensor::accel_calibrated_ok_all(), AccelCalibrator::accept_result(), AP_Compass_MMC3416::accumulate_field(), AP_AHRS::add_trim(), AC_Avoid::adjust_velocity(), AC_WPNav::advance_spline_target_along_track(), AC_WPNav::advance_wp_target_along_track(), NavEKF2_core::alignMagStateDeclination(), NavEKF3_core::alignMagStateDeclination(), AC_AttitudeControl::ang_vel_limit(), AC_AttitudeControl::ang_vel_to_euler_rate(), AP_AHRS::AP_AHRS(), AC_AttitudeControl::attitude_controller_run_quat(), AP_Landing_Deepstall::build_approach_path(), AP_Mount_Backend::calc_angle_to_location(), AP_GPS::calc_blended_state(), AC_Loiter::calc_desired_velocity(), CompassCalibrator::calc_ellipsoid_jacob(), AccelCalibrator::calc_jacob(), CompassCalibrator::calc_residual(), AccelCalibrator::calc_residual(), CompassCalibrator::calc_sphere_jacob(), AP_AHRS::calc_trig(), AP_InertialSensor::calc_vibration_and_clipping(), NavEKF2_core::calcEarthRateNED(), NavEKF3_core::calcEarthRateNED(), NavEKF2_core::calcGpsGoodToAlign(), NavEKF3_core::calcGpsGoodToAlign(), SoloGimbalEKF::calcMagHeadingInnov(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), NavEKF2_core::calcQuatAndFieldStates(), NavEKF3_core::calcQuatAndFieldStates(), NavEKF2_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::calcRotVecVariances(), SITL::Frame::calculate_forces(), SITL::Submarine::calculate_forces(), SITL::Plane::calculate_forces(), Compass::calculate_heading(), AC_WPNav::calculate_wp_leash_length(), check_path(), NavEKF3_core::checkAttitudeAlignmentStatus(), Matrix3< float >::coly(), Compass::consistent(), AC_PrecLand::construct_pos_meas_using_rangefinder(), AP_Mount_Alexmos::control_axis(), NavEKF2_core::controlMagYawReset(), NavEKF3_core::controlMagYawReset(), SITL::SITL::convert_earth_frame(), AP_Compass_Backend::correct_field(), AP_Beacon_Backend::correct_for_orient_yaw(), NavEKF2_core::correctDeltaAngle(), NavEKF2_core::correctEkfOriginHeight(), NavEKF3_core::correctEkfOriginHeight(), NavEKF3_core::CovarianceInit(), NavEKF2_core::CovariancePrediction(), AC_PosControl::desired_accel_to_vel(), AC_PosControl::desired_vel_to_pos(), NavEKF2_core::detectFlight(), NavEKF3_core::detectFlight(), display_offsets_and_scaling(), Vector3< int32_t >::distance_squared(), AP_AHRS_DCM::drift_correction(), AP_AHRS_DCM::estimate_wind(), NavEKF2_core::EstimateTerrainOffset(), NavEKF3_core::EstimateTerrainOffset(), AC_AttitudeControl::euler_accel_limit(), AC_AttitudeControl::euler_rate_to_ang_vel(), AP_GPS_Backend::fill_3d_velocity(), SITL::Aircraft::fill_fdm(), CompassCalibrator::fit_acceptable(), Quaternion::from_axis_angle(), Matrix3< float >::from_axis_angle(), Quaternion::from_axis_angle_fast(), Quaternion::from_rotation_matrix(), NavEKF2_core::FuseAirspeed(), NavEKF3_core::FuseAirspeed(), NavEKF3_core::FuseBodyVel(), SoloGimbalEKF::fuseCompass(), NavEKF2_core::FuseDeclination(), NavEKF3_core::FuseDeclination(), NavEKF2_core::fuseEulerYaw(), NavEKF3_core::fuseEulerYaw(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), NavEKF2_core::FuseRngBcn(), NavEKF3_core::FuseRngBcn(), NavEKF2_core::FuseRngBcnStatic(), NavEKF3_core::FuseRngBcnStatic(), NavEKF2_core::FuseSideslip(), NavEKF3_core::FuseSideslip(), SoloGimbalEKF::fuseVelocity(), NavEKF2_core::FuseVelPosNED(), NavEKF3_core::FuseVelPosNED(), AP_InertialSensor_SITL::generate_accel(), AP_InertialSensor_SITL::generate_gyro(), SoloGimbal_Parameters::get_accel_bias(), SoloGimbal_Parameters::get_accel_gain(), get_bearing_cd(), AC_Circle::get_closest_point_on_circle(), AR_AttitudeControl::get_forward_speed(), SoloGimbal_Parameters::get_gyro_bias(), get_horizontal_distance_cm(), AC_PosControl::get_horizontal_error(), SoloGimbal_Parameters::get_joint_bias(), AP_Follow::get_offsets_ned(), AP_AHRS_NavEKF::get_relative_position_NED_home(), AP_AHRS_NavEKF::get_relative_position_NED_origin(), AccelCalibrator::get_sample_corrected(), AP_YawController::get_servo_out(), AC_PosControl::get_stopping_point_xy(), AP_Follow::get_target_location_and_velocity(), IRLock::get_unit_vector_body(), Location_Class::get_vector_from_origin_NEU(), AC_WPNav::get_vector_NEU(), AP_InertialSensor::get_vibration_levels(), SoaringController::get_wind_corrected_drift(), AC_WPNav::get_wp_destination(), AC_WPNav::get_wp_distance_to_destination(), NavEKF3_core::getBodyFrameOdomDebug(), SoloGimbalEKF::getDebug(), NavEKF2_core::getEulerAngles(), NavEKF3_core::getEulerAngles(), SITL::Plane::getForce(), NavEKF2_core::getGyroScaleErrorPercentage(), NavEKF2_core::getInnovations(), NavEKF3_core::getInnovations(), NavEKF2_core::getLLH(), NavEKF3_core::getLLH(), NavEKF2_core::getPosNE(), NavEKF3_core::getPosNE(), SITL::Plane::getTorque(), NavEKF2_core::getVariances(), NavEKF3_core::getVariances(), NavEKF2_core::getWind(), NavEKF3_core::getWind(), AP_AHRS_NavEKF::groundspeed_vector(), AP_AHRS::groundspeed_vector(), AP_GPS_MAV::handle_msg(), AP_Follow::handle_msg(), Matrix3< float >::identity(), AC_AttitudeControl::inertial_frame_reset(), AC_Circle::init(), AC_WPNav::init_brake_target(), AC_Circle::init_start_angle(), AC_Loiter::init_target(), AC_PosControl::init_vel_controller_xyz(), AC_PosControl::init_xy_controller(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), NavEKF3_core::initialiseQuatCovariances(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(), 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_rate_bf_roll_pitch_yaw(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(), AC_AttitudeControl::input_shaping_rate_predictor(), AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors(), AP_InertialSensor::is_still(), Location_Class::Location_Class(), AP_InertialSensor_Backend::log_accel_raw(), AP_InertialSensor_Backend::log_gyro_raw(), AP_Airspeed::log_mavlink_send(), DataFlash_Class::Log_Write_AHRS2(), DataFlash_Class::Log_Write_Attitude(), DataFlash_Class::Log_Write_AttitudeView(), DataFlash_Class::Log_Write_Beacon(), DataFlash_Class::Log_Write_Compass_instance(), DataFlash_Class::Log_Write_IMU_instance(), DataFlash_Class::Log_Write_IMUDT_instance(), DataFlash_Class::Log_Write_POS(), DataFlash_Class::Log_Write_Rate(), DataFlash_Class::Log_Write_SRTL(), DataFlash_Class::Log_Write_Vibration(), DataFlash_Class::Log_Write_VisualOdom(), loop(), Vector3< int32_t >::mul_rowcol(), Matrix3< float >::mul_transpose(), Matrix3< float >::mulXY(), Vector3< int32_t >::operator!=(), Vector3< int32_t >::operator%(), Vector3< int32_t >::operator*(), Matrix3< float >::operator*(), Vector3< int32_t >::operator+(), Vector3< int32_t >::operator+=(), Vector3< int32_t >::operator-(), Vector3< int32_t >::operator-=(), Vector3< int32_t >::operator==(), AP_Mount_Alexmos::parse_body(), AP_Mount_SToRM32_serial::parse_reply(), AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(), AP_Landing_Deepstall::predict_travel_distance(), SoloGimbalEKF::predictCovariance(), SoloGimbalEKF::predictStates(), print_vector(), AP_GPS_SBF::process_message(), CompassLearn::process_sample(), AC_AttitudeControl::rate_bf_pitch_target(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), AC_AttitudeControl_Sub::rate_controller_run(), AC_AttitudeControl_Heli::rate_controller_run(), AC_AttitudeControl_Multi::rate_controller_run(), NavEKF2_core::readRngBcnData(), NavEKF3_core::readRngBcnData(), NavEKF2_core::realignYawGPS(), NavEKF3_core::realignYawGPS(), SITL::XPlane::receive_data(), SITL::CRRCSim::recv_fdm(), SITL::FlightAxis::report_FPS(), AP_AHRS_DCM::reset(), NavEKF2_core::ResetPosition(), NavEKF3_core::ResetPosition(), NavEKF2_core::ResetVelocity(), NavEKF3_core::ResetVelocity(), Matrix3< float >::rotate(), Quaternion::rotate_fast(), Vector3< int32_t >::rotate_inverse(), Quaternion::rotation_matrix(), Quaternion::rotation_matrix_norm(), AC_PrecLand::run_estimator(), AC_PrecLand::run_output_prediction(), run_test(), AC_PosControl::run_xy_controller(), SoloGimbalEKF::RunEKF(), AP_InertialSensor::BatchSampler::sample(), NavEKF3_core::SelectBodyOdomFusion(), NavEKF2_core::selectHeightForFusion(), NavEKF3_core::selectHeightForFusion(), NavEKF2_core::SelectMagFusion(), NavEKF3_core::SelectMagFusion(), NavEKF3_core::SelectRngBcnFusion(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), GCS_MAVLINK::send_ahrs(), GCS_MAVLINK::send_ahrs2(), GCS_MAVLINK::send_attitude(), AP_ADSB::send_dynamic_out(), GCS_MAVLINK::send_global_position_int(), GCS_MAVLINK::send_local_position(), Compass::send_mag_cal_progress(), Compass::send_mag_cal_report(), AP_Param::send_parameter(), GCS_MAVLINK::send_raw_imu(), SITL::ADSB::send_report(), SITL::Gimbal::send_report(), GCS_MAVLINK::send_sensor_offsets(), GCS_MAVLINK::send_vibration(), CompassCalibrator::CompassSample::set(), SoloGimbal_Parameters::set_accel_bias(), SoloGimbal_Parameters::set_accel_gain(), AP_Mount_Backend::set_angle_targets(), AC_PosControl::set_desired_accel_xy(), AC_PosControl::set_desired_velocity_xy(), SoloGimbal_Parameters::set_gyro_bias(), AC_PID_2D::set_input(), AC_PI_2D::set_input(), AC_PID_2D::set_integrator(), AC_PI_2D::set_integrator(), AC_WPNav::set_spline_origin_and_destination(), AP_AHRS::set_trim(), AC_WPNav::set_wp_destination_NED(), AC_WPNav::set_wp_origin_and_destination(), AC_PosControl::set_xy_target(), NavEKF2_core::setAidingMode(), NavEKF3_core::setAidingMode(), AP_GPS::setHIL(), NavEKF2_core::setWindMagStateLearningMode(), NavEKF3_core::setWindMagStateLearningMode(), AC_PosControl::shift_pos_xy_target(), SITL::Aircraft::smooth_sensors(), AC_Loiter::soften_for_landing(), AC_PosControl::sqrt_controller(), AP_Mount_Servo::stabilize(), AP_Mount_SToRM32_serial::status_msg(), AP_Mount_SToRM32::status_msg(), AP_Mount_Servo::status_msg(), AP_Mount_Alexmos::status_msg(), test_euler(), test_frame_transforms(), test_matrix_rotate(), AC_AttitudeControl::thrust_heading_rotation_angles(), AP_Compass_QMC5883L::timer(), AP_Compass_MMC3416::timer(), AP_OpticalFlow_Pixart::timer(), CompassLearn::update(), AP_OpticalFlow_SITL::update(), Airspeed_Calibration::update(), AP_Mount_SToRM32_serial::update(), SITL::Gimbal::update(), AP_Mount_SToRM32::update(), SITL::Helicopter::update(), SITL::SimRover::update(), SITL::SingleCopter::update(), SITL::FlightAxis::update(), AP_Mount_Servo::update(), AC_Circle::update(), AC_Sprayer::update(), AP_AHRS_NavEKF::update(), AC_AttitudeControl::update_ang_vel_target_from_att_error(), AP_AHRS::update_AOA_SSA(), AP_Beacon::update_boundary_points(), CompassCalibrator::update_completion_mask(), AP_AHRS_NavEKF::update_DCM(), SITL::Aircraft::update_dynamics(), AP_AHRS_NavEKF::update_EKF2(), AP_AHRS_NavEKF::update_EKF3(), SITL::Aircraft::update_position(), AP_Mount_Backend::update_targets_from_rc(), AP_AHRS::update_trig(), SITL::Vicon::update_vicon_position_estimate(), NavEKF2_core::UpdateStrapdownEquationsNED(), NavEKF3_core::UpdateStrapdownEquationsNED(), AP_Landing::wind_alignment(), AC_PosControl::write_log(), NavEKF2_core::writeOptFlowMeas(), and NavEKF3_core::writeOptFlowMeas().
T Vector3< T >::z |
Definition at line 67 of file vector3.h.
Referenced by AP_InertialSensor::_acal_save_calibrations(), AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(), AP_InertialSensor::_calculate_trim(), AP_Compass_AK8963::_make_factory_sensitivity_adjustment(), AP_GPS_ERB::_parse_gps(), AP_GPS_UBLOX::_parse_gps(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_Compass_LSM303D::_update(), AP_Compass_MAG3110::_update(), HALSITL::SITL_State::_update_gps(), HALSITL::SITL_State::_update_rangefinder(), AP_InertialSensor::accel_calibrated_ok_all(), AccelCalibrator::accept_result(), AP_Compass_MMC3416::accumulate_field(), AC_PosControl::add_takeoff_climb_rate(), AC_WPNav::advance_spline_target_along_track(), AC_WPNav::advance_wp_target_along_track(), AC_AttitudeControl::ang_vel_limit(), AC_AttitudeControl::ang_vel_to_euler_rate(), AC_AttitudeControl::attitude_controller_run_quat(), AP_Mount_Backend::calc_angle_to_location(), CompassCalibrator::calc_ellipsoid_jacob(), AccelCalibrator::calc_jacob(), CompassCalibrator::calc_residual(), AccelCalibrator::calc_residual(), CompassCalibrator::calc_sphere_jacob(), AP_AHRS::calc_trig(), AP_InertialSensor::calc_vibration_and_clipping(), NavEKF2_core::calcEarthRateNED(), NavEKF3_core::calcEarthRateNED(), NavEKF2_core::calcFiltBaroOffset(), NavEKF3_core::calcFiltBaroOffset(), NavEKF2_core::calcGpsGoodToAlign(), NavEKF3_core::calcGpsGoodToAlign(), NavEKF2_core::calcOutputStates(), NavEKF3_core::calcOutputStates(), NavEKF2_core::calcQuatAndFieldStates(), NavEKF3_core::calcQuatAndFieldStates(), NavEKF2_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::CalcRangeBeaconPosDownOffset(), NavEKF3_core::calcRotVecVariances(), SITL::Submarine::calculate_buoyancy_acceleration(), SITL::Frame::calculate_forces(), SITL::Submarine::calculate_forces(), SITL::Plane::calculate_forces(), Compass::calculate_heading(), AC_WPNav::calculate_wp_leash_length(), check_path(), Matrix3< float >::colz(), NavEKF2_core::ConstrainStates(), NavEKF3_core::ConstrainStates(), AC_PrecLand::construct_pos_meas_using_rangefinder(), AP_Mount_Alexmos::control_axis(), NavEKF2_core::controlMagYawReset(), NavEKF3_core::controlMagYawReset(), SITL::SITL::convert_earth_frame(), AP_Compass_Backend::correct_field(), AP_Beacon_Backend::correct_for_orient_yaw(), NavEKF2_core::correctDeltaAngle(), NavEKF2_core::correctDeltaVelocity(), NavEKF2_core::correctEkfOriginHeight(), NavEKF3_core::correctEkfOriginHeight(), NavEKF3_core::CovarianceInit(), NavEKF2_core::CovariancePrediction(), NavEKF3_core::CovariancePrediction(), NavEKF2_core::detectFlight(), NavEKF3_core::detectFlight(), display_offsets_and_scaling(), Vector3< int32_t >::distance_squared(), AP_AHRS_DCM::drift_correction(), AP_AHRS_DCM::drift_correction_yaw(), AP_AHRS_DCM::estimate_wind(), NavEKF2_core::EstimateTerrainOffset(), NavEKF3_core::EstimateTerrainOffset(), AC_AttitudeControl::euler_accel_limit(), AC_AttitudeControl::euler_rate_to_ang_vel(), SITL::Aircraft::extrapolate_sensors(), AP_GPS_Backend::fill_3d_velocity(), SITL::Aircraft::fill_fdm(), CompassCalibrator::fit_acceptable(), Quaternion::from_axis_angle(), Matrix3< float >::from_axis_angle(), Quaternion::from_axis_angle_fast(), Quaternion::from_rotation_matrix(), NavEKF2_core::FuseAirspeed(), NavEKF3_core::FuseAirspeed(), NavEKF3_core::FuseBodyVel(), SoloGimbalEKF::fuseCompass(), NavEKF2_core::fuseEulerYaw(), NavEKF3_core::fuseEulerYaw(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), NavEKF2_core::FuseRngBcn(), NavEKF3_core::FuseRngBcn(), NavEKF2_core::FuseRngBcnStatic(), NavEKF3_core::FuseRngBcnStatic(), NavEKF2_core::FuseSideslip(), NavEKF3_core::FuseSideslip(), NavEKF2_core::FuseVelPosNED(), NavEKF3_core::FuseVelPosNED(), AP_InertialSensor_SITL::generate_accel(), AP_InertialSensor_SITL::generate_gyro(), SoloGimbal_Parameters::get_accel_bias(), SoloGimbal_Parameters::get_accel_gain(), AC_PosControl::get_alt_error(), AC_PosControl::get_alt_target(), AC_Circle::get_closest_point_on_circle(), SoloGimbal_Parameters::get_gyro_bias(), SoloGimbal_Parameters::get_joint_bias(), AP_Follow::get_offsets_ned(), AP_AHRS_NavEKF::get_position(), AP_AHRS_NavEKF::get_relative_position_NED_home(), AP_AHRS_NavEKF::get_relative_position_NED_origin(), AccelCalibrator::get_sample_corrected(), AP_YawController::get_servo_out(), AC_PosControl::get_stopping_point_z(), AP_Follow::get_target_location_and_velocity(), IRLock::get_unit_vector_body(), Location_Class::get_vector_from_origin_NEU(), AC_WPNav::get_vector_NEU(), AC_PosControl::get_vel_target_z(), AP_InertialSensor::get_vibration_levels(), AC_WPNav::get_wp_destination(), AC_WPNav::get_yaw(), NavEKF2_core::getAccelNED(), NavEKF3_core::getAccelNED(), NavEKF3_core::getBodyFrameOdomDebug(), AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(), SoloGimbalEKF::getDebug(), NavEKF2_core::getEulerAngles(), NavEKF3_core::getEulerAngles(), NavEKF2_core::getFlowDebug(), NavEKF3_core::getFlowDebug(), SITL::Plane::getForce(), NavEKF2_core::getGyroScaleErrorPercentage(), NavEKF2_core::getHAGL(), NavEKF3_core::getHAGL(), NavEKF2_core::getInnovations(), NavEKF3_core::getInnovations(), NavEKF2_core::getLLH(), NavEKF3_core::getLLH(), NavEKF2_core::getPosD(), NavEKF3_core::getPosD(), NavEKF2_core::getPosDownDerivative(), NavEKF3_core::getPosDownDerivative(), SITL::Plane::getTorque(), NavEKF2_core::getVariances(), NavEKF3_core::getVariances(), NavEKF2_core::getWind(), NavEKF3_core::getWind(), SITL::Aircraft::hagl(), AP_GPS_MAV::handle_msg(), AP_Follow::handle_msg(), Matrix3< float >::identity(), AC_AttitudeControl::inertial_frame_reset(), Airspeed_Calibration::init(), AC_Circle::init(), AC_PosControl::init_takeoff(), AC_PosControl::init_vel_controller_xyz(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), NavEKF3_core::initialiseQuatCovariances(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(), 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_rate_bf_roll_pitch_yaw(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(), AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(), AP_Arming::ins_checks(), AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors(), AP_InertialSensor::is_still(), Location_Class::Location_Class(), AP_InertialSensor_Backend::log_accel_raw(), AP_InertialSensor_Backend::log_gyro_raw(), AP_Airspeed::log_mavlink_send(), DataFlash_Class::Log_Write_AHRS2(), DataFlash_Class::Log_Write_Attitude(), DataFlash_Class::Log_Write_AttitudeView(), DataFlash_Class::Log_Write_Beacon(), DataFlash_Class::Log_Write_Compass_instance(), DataFlash_Class::Log_Write_GPS(), DataFlash_Class::Log_Write_IMU_instance(), DataFlash_Class::Log_Write_IMUDT_instance(), DataFlash_Class::Log_Write_POS(), DataFlash_Class::Log_Write_Rate(), DataFlash_Class::Log_Write_SRTL(), DataFlash_Class::Log_Write_Vibration(), DataFlash_Class::Log_Write_VisualOdom(), loop(), Vector3< int32_t >::mul_rowcol(), Matrix3< float >::mul_transpose(), Matrix3< float >::mulXY(), Vector3< int32_t >::operator!=(), Vector3< int32_t >::operator%(), Vector3< int32_t >::operator*(), Matrix3< float >::operator*(), Vector3< int32_t >::operator+(), Vector3< int32_t >::operator+=(), Vector3< int32_t >::operator-(), Vector3< int32_t >::operator-=(), Vector3< int32_t >::operator==(), AP_Mount_Alexmos::parse_body(), AP_Mount_SToRM32_serial::parse_reply(), AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(), SoloGimbalEKF::predictCovariance(), SoloGimbalEKF::predictStates(), print_vector(), AP_GPS_NOVA::process_message(), AP_GPS_GSOF::process_message(), AP_GPS_SBF::process_message(), CompassLearn::process_sample(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), AC_AttitudeControl::rate_bf_yaw_target(), AC_AttitudeControl_Sub::rate_controller_run(), AC_AttitudeControl_Heli::rate_controller_run(), AC_AttitudeControl_Multi::rate_controller_run(), NavEKF2_core::readGpsData(), NavEKF3_core::readGpsData(), NavEKF2_core::realignYawGPS(), NavEKF3_core::realignYawGPS(), SITL::XPlane::receive_data(), NavEKF2_core::recordMagReset(), NavEKF3_core::recordMagReset(), SITL::CRRCSim::recv_fdm(), AC_PosControl::relax_alt_hold_controllers(), SITL::FlightAxis::report_FPS(), AP_AHRS_DCM::reset(), NavEKF2_core::ResetHeight(), NavEKF3_core::ResetHeight(), NavEKF2_core::resetHeightDatum(), NavEKF3_core::resetHeightDatum(), Matrix3< float >::rotate(), Quaternion::rotate_fast(), Vector3< int32_t >::rotate_inverse(), Quaternion::rotation_matrix(), Quaternion::rotation_matrix_norm(), AC_PrecLand::run_estimator(), run_test(), AC_PosControl::run_z_controller(), SoloGimbalEKF::RunEKF(), AP_InertialSensor::BatchSampler::sample(), NavEKF3_core::SelectBodyOdomFusion(), NavEKF2_core::SelectFlowFusion(), NavEKF3_core::SelectFlowFusion(), NavEKF2_core::selectHeightForFusion(), NavEKF3_core::selectHeightForFusion(), NavEKF2_core::SelectMagFusion(), NavEKF3_core::SelectMagFusion(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), GCS_MAVLINK::send_ahrs(), GCS_MAVLINK::send_ahrs2(), GCS_MAVLINK::send_attitude(), AP_ADSB::send_dynamic_out(), GCS_MAVLINK::send_global_position_int(), GCS_MAVLINK::send_local_position(), Compass::send_mag_cal_progress(), Compass::send_mag_cal_report(), AP_Param::send_parameter(), GCS_MAVLINK::send_raw_imu(), SITL::ADSB::send_report(), SITL::Gimbal::send_report(), GCS_MAVLINK::send_sensor_offsets(), GCS_MAVLINK::send_vibration(), CompassCalibrator::CompassSample::set(), SoloGimbal_Parameters::set_accel_bias(), SoloGimbal_Parameters::set_accel_gain(), AC_PosControl::set_alt_target(), AC_PosControl_Sub::set_alt_target_from_climb_rate(), AC_PosControl::set_alt_target_from_climb_rate(), AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(), AC_PosControl::set_alt_target_from_climb_rate_ff(), AC_PosControl::set_alt_target_to_current_alt(), AC_PosControl::set_alt_target_with_slew(), AP_Mount_Backend::set_angle_targets(), AC_PosControl::set_desired_velocity_z(), SoloGimbal_Parameters::set_gyro_bias(), AC_PosControl::set_pos_target(), AC_WPNav::set_spline_destination(), AC_WPNav::set_wp_destination(), AC_WPNav::set_wp_destination_NED(), AC_WPNav::set_wp_origin_and_destination(), AC_AttitudeControl::set_yaw_target_to_current_heading(), NavEKF2_core::setAidingMode(), NavEKF3_core::setAidingMode(), NavEKF2_core::setOrigin(), NavEKF3_core::setOrigin(), NavEKF2_core::setWindMagStateLearningMode(), NavEKF3_core::setWindMagStateLearningMode(), AC_PosControl::shift_alt_target(), SITL::Aircraft::smooth_sensors(), AC_PosControl::sqrt_controller(), AP_Mount_Servo::stabilize(), AP_Mount_SToRM32_serial::status_msg(), AP_Mount_SToRM32::status_msg(), AP_Mount_Servo::status_msg(), AP_Mount_Alexmos::status_msg(), NavEKF2_core::StoreOutputReset(), NavEKF3_core::StoreOutputReset(), test_euler(), test_frame_transforms(), test_matrix_rotate(), AC_AttitudeControl::thrust_heading_rotation_angles(), AP_Compass_QMC5883L::timer(), AP_Compass_MMC3416::timer(), CompassLearn::update(), AP_OpticalFlow_SITL::update(), Airspeed_Calibration::update(), AP_Mount_SToRM32_serial::update(), SITL::Gimbal::update(), AP_Mount_SToRM32::update(), AP_ICEngine::update(), SITL::Helicopter::update(), SITL::Balloon::update(), SITL::SimRover::update(), SITL::SingleCopter::update(), SITL::FlightAxis::update(), AP_Mount_Servo::update(), AC_Circle::update(), AC_PrecLand::update(), AP_TECS::update_50hz(), AC_AttitudeControl::update_ang_vel_target_from_att_error(), AP_AHRS::update_AOA_SSA(), AP_Airspeed::update_calibration(), CompassCalibrator::update_completion_mask(), AP_AHRS_NavEKF::update_DCM(), SITL::Aircraft::update_dynamics(), AP_AHRS_NavEKF::update_EKF2(), AP_AHRS_NavEKF::update_EKF3(), SITL::Aircraft::update_mag_field_bf(), SITL::Aircraft::update_position(), AP_Landing_Deepstall::update_steering(), AP_Mount_Backend::update_targets_from_rc(), AC_PosControl::update_vel_controller_xyz(), SITL::Vicon::update_vicon_position_estimate(), NavEKF2::updateLaneSwitchYawResetData(), NavEKF3::updateLaneSwitchYawResetData(), NavEKF2_core::UpdateStrapdownEquationsNED(), NavEKF3_core::UpdateStrapdownEquationsNED(), GCS_MAVLINK::vfr_hud_climbrate(), NavEKF2_core::writeOptFlowMeas(), and NavEKF3_core::writeOptFlowMeas().