APM:Libraries
|
#include <AP_InertialSensor.h>
Classes | |
class | BatchSampler |
struct | PeakHoldState |
Public Types | |
enum | Gyro_Calibration_Timing { GYRO_CAL_NEVER = 0, GYRO_CAL_STARTUP_ONLY = 1 } |
enum | IMU_SENSOR_TYPE { IMU_SENSOR_TYPE_ACCEL = 0, IMU_SENSOR_TYPE_GYRO = 1 } |
Public Member Functions | |
AP_InertialSensor () | |
AP_InertialSensor (const AP_InertialSensor &other)=delete | |
AP_InertialSensor & | operator= (const AP_InertialSensor &)=delete |
void | init (uint16_t sample_rate_hz) |
uint8_t | register_gyro (uint16_t raw_sample_rate_hz, uint32_t id) |
uint8_t | register_accel (uint16_t raw_sample_rate_hz, uint32_t id) |
void | periodic () |
bool | calibrate_trim (float &trim_roll, float &trim_pitch) |
bool | calibrating () const |
calibrating - returns true if the gyros or accels are currently being calibrated More... | |
void | init_gyro (void) |
const Vector3f & | get_gyro (uint8_t i) const |
const Vector3f & | get_gyro (void) const |
const Vector3f & | get_gyro_offsets (uint8_t i) const |
const Vector3f & | get_gyro_offsets (void) const |
bool | get_delta_angle (uint8_t i, Vector3f &delta_angle) const |
bool | get_delta_angle (Vector3f &delta_angle) const |
float | get_delta_angle_dt (uint8_t i) const |
float | get_delta_angle_dt () const |
bool | get_delta_velocity (uint8_t i, Vector3f &delta_velocity) const |
bool | get_delta_velocity (Vector3f &delta_velocity) const |
float | get_delta_velocity_dt (uint8_t i) const |
float | get_delta_velocity_dt () const |
const Vector3f & | get_accel (uint8_t i) const |
const Vector3f & | get_accel (void) const |
uint32_t | get_gyro_error_count (uint8_t i) const |
uint32_t | get_accel_error_count (uint8_t i) const |
bool | get_gyro_health (uint8_t instance) const |
bool | get_gyro_health (void) const |
bool | get_gyro_health_all (void) const |
uint8_t | get_gyro_count (void) const |
bool | gyro_calibrated_ok (uint8_t instance) const |
bool | gyro_calibrated_ok_all () const |
bool | use_gyro (uint8_t instance) const |
Gyro_Calibration_Timing | gyro_calibration_timing () |
bool | get_accel_health (uint8_t instance) const |
bool | get_accel_health (void) const |
bool | get_accel_health_all (void) const |
uint8_t | get_accel_count (void) const |
bool | accel_calibrated_ok_all () const |
bool | use_accel (uint8_t instance) const |
uint16_t | get_gyro_rate_hz (uint8_t instance) const |
uint16_t | get_accel_rate_hz (uint8_t instance) const |
const Vector3f & | get_accel_offsets (uint8_t i) const |
const Vector3f & | get_accel_offsets (void) const |
const Vector3f & | get_accel_scale (uint8_t i) const |
const Vector3f & | get_accel_scale (void) const |
const Vector3f & | get_imu_pos_offset (uint8_t instance) const |
const Vector3f & | get_imu_pos_offset (void) const |
float | get_temperature (uint8_t instance) const |
float | get_delta_time () const |
float | get_gyro_drift_rate (void) const |
void | update (void) |
void | wait_for_sample (void) |
void | set_board_orientation (enum Rotation orientation, Matrix3f *custom_rotation=nullptr) |
uint16_t | get_sample_rate (void) const |
float | get_loop_delta_t (void) const |
bool | healthy (void) const |
uint8_t | get_primary_accel (void) const |
uint8_t | get_primary_gyro (void) const |
void | set_hil_mode (void) |
uint8_t | get_gyro_filter_hz (void) const |
uint8_t | get_accel_filter_hz (void) const |
void | set_log_raw_bit (uint32_t log_raw_bit) |
void | calc_vibration_and_clipping (uint8_t instance, const Vector3f &accel, float dt) |
Vector3f | get_vibration_levels () const |
Vector3f | get_vibration_levels (uint8_t instance) const |
uint32_t | get_accel_clip_count (uint8_t instance) const |
bool | is_still () |
void | set_accel (uint8_t instance, const Vector3f &accel) |
void | set_gyro (uint8_t instance, const Vector3f &gyro) |
void | set_delta_time (float delta_time) |
void | set_delta_velocity (uint8_t instance, float deltavt, const Vector3f &deltav) |
void | set_delta_angle (uint8_t instance, const Vector3f &deltaa, float deltaat) |
AuxiliaryBus * | get_auxiliary_bus (int16_t backend_id) |
AuxiliaryBus * | get_auxiliary_bus (int16_t backend_id, uint8_t instance) |
void | detect_backends (void) |
void | set_accel_peak_hold (uint8_t instance, const Vector3f &accel) |
float | get_accel_peak_hold_neg_x () const |
AP_AccelCal * | get_acal () const |
bool | get_fixed_mount_accel_cal_sample (uint8_t sample_num, Vector3f &ret) const |
bool | get_primary_accel_cal_sample_avg (uint8_t sample_num, Vector3f &ret) const |
bool | get_new_trim (float &trim_roll, float &trim_pitch) |
void | acal_init () |
void | acal_update () |
MAV_RESULT | simple_accel_cal () |
bool | accel_cal_requires_reboot () const |
uint32_t | get_last_update_usec (void) const |
Static Public Member Functions | |
static AP_InertialSensor * | get_instance () |
Public Attributes | |
BatchSampler | batchsampler {*this} |
float | delta_time |
Static Public Attributes | |
static const struct AP_Param::GroupInfo | var_info [] |
Private Member Functions | |
bool | _add_backend (AP_InertialSensor_Backend *backend) |
void | _start_backends () |
AP_InertialSensor_Backend * | _find_backend (int16_t backend_id, uint8_t instance) |
void | _init_gyro () |
bool | _calculate_trim (const Vector3f &accel_sample, float &trim_roll, float &trim_pitch) |
void | _save_gyro_calibration () |
void | _acal_save_calibrations () |
void | _acal_event_failure () |
AccelCalibrator * | _acal_get_calibrator (uint8_t i) |
Static Private Attributes | |
static AP_InertialSensor * | _s_instance = nullptr |
Friends | |
class | AP_InertialSensor_Backend |
Definition at line 47 of file AP_InertialSensor.h.
Enumerator | |
---|---|
GYRO_CAL_NEVER | |
GYRO_CAL_STARTUP_ONLY |
Definition at line 60 of file AP_InertialSensor.h.
Enumerator | |
---|---|
IMU_SENSOR_TYPE_ACCEL | |
IMU_SENSOR_TYPE_GYRO |
Definition at line 278 of file AP_InertialSensor.h.
AP_InertialSensor::AP_InertialSensor | ( | ) |
Definition at line 455 of file AP_InertialSensor.cpp.
Referenced by get_instance().
|
delete |
|
privatevirtual |
Reimplemented from AP_AccelCal_Client.
Definition at line 1749 of file AP_InertialSensor.cpp.
|
inlineprivatevirtual |
Implements AP_AccelCal_Client.
Definition at line 569 of file AP_InertialSensor.h.
|
privatevirtual |
Implements AP_AccelCal_Client.
Definition at line 1685 of file AP_InertialSensor.cpp.
|
private |
Definition at line 651 of file AP_InertialSensor.cpp.
Referenced by detect_backends().
|
private |
Definition at line 882 of file AP_InertialSensor.cpp.
Referenced by calibrate_trim().
|
private |
Definition at line 586 of file AP_InertialSensor.cpp.
Referenced by get_auxiliary_bus().
|
private |
Definition at line 1078 of file AP_InertialSensor.cpp.
Referenced by init_gyro().
|
private |
Definition at line 1217 of file AP_InertialSensor.cpp.
Referenced by init_gyro().
|
private |
Definition at line 563 of file AP_InertialSensor.cpp.
Referenced by init().
void AP_InertialSensor::acal_init | ( | ) |
Definition at line 1657 of file AP_InertialSensor.cpp.
Referenced by GCS_MAVLINK::_handle_command_preflight_calibration(), and get_acal().
void AP_InertialSensor::acal_update | ( | ) |
Definition at line 1669 of file AP_InertialSensor.cpp.
Referenced by get_acal().
|
inline |
Definition at line 273 of file AP_InertialSensor.h.
Referenced by AP_Arming::ins_checks().
bool AP_InertialSensor::accel_calibrated_ok_all | ( | ) | const |
Definition at line 1023 of file AP_InertialSensor.cpp.
Referenced by get_accel_count(), and AP_Arming::ins_checks().
void AP_InertialSensor::calc_vibration_and_clipping | ( | uint8_t | instance, |
const Vector3f & | accel, | ||
float | dt | ||
) |
Definition at line 1593 of file AP_InertialSensor.cpp.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and set_log_raw_bit().
bool AP_InertialSensor::calibrate_trim | ( | float & | trim_roll, |
float & | trim_pitch | ||
) |
|
inline |
calibrating - returns true if the gyros or accels are currently being calibrated
Definition at line 86 of file AP_InertialSensor.h.
void AP_InertialSensor::detect_backends | ( | void | ) |
Definition at line 668 of file AP_InertialSensor.cpp.
Referenced by _start_backends(), get_auxiliary_bus(), and AP_Compass_AK8963::probe_mpu9250().
|
inline |
Definition at line 252 of file AP_InertialSensor.h.
Referenced by GCS_MAVLINK::_handle_command_preflight_calibration(), and GCS_MAVLINK::handle_command_ack().
|
inline |
Fetch the current accelerometer values
Definition at line 124 of file AP_InertialSensor.h.
Referenced by AP_AHRS_DCM::drift_correction(), AP_YawController::get_servo_out(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), AP_Arming::ins_checks(), DataFlash_Class::Log_Write_IMU_instance(), AP_AHRS_DCM::reset(), run_test(), GCS_MAVLINK::send_raw_imu(), AP_TECS::update_50hz(), AP_AHRS_NavEKF::update_EKF2(), and AP_AHRS_NavEKF::update_EKF3().
|
inline |
Definition at line 125 of file AP_InertialSensor.h.
Referenced by _init_gyro(), calibrate_trim(), get_accel(), get_delta_velocity(), and simple_accel_cal().
uint32_t AP_InertialSensor::get_accel_clip_count | ( | uint8_t | instance | ) | const |
Definition at line 907 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels(), DataFlash_Class::Log_Write_Vibration(), and GCS_MAVLINK::send_vibration().
|
inline |
Definition at line 143 of file AP_InertialSensor.h.
Referenced by _acal_get_calibrator(), _start_backends(), accel_calibrated_ok_all(), AP_AHRS_DCM::drift_correction(), get_accel_clip_count(), get_accel_health_all(), init(), NavEKF3::InitialiseFilter(), NavEKF2::InitialiseFilter(), AP_Arming::ins_checks(), DataFlash_Class::Log_Write_IMU(), DataFlash_Class::Log_Write_IMUDT(), NavEKF2_core::readDeltaVelocity(), NavEKF3_core::readDeltaVelocity(), run_test(), GCS_MAVLINK::send_raw_imu(), setup(), simple_accel_cal(), AP_AHRS_NavEKF::update_EKF2(), and AP_AHRS_NavEKF::update_EKF3().
|
inline |
Definition at line 128 of file AP_InertialSensor.h.
Referenced by DataFlash_Class::Log_Write_IMU_instance().
|
inline |
Definition at line 213 of file AP_InertialSensor.h.
|
inline |
Definition at line 140 of file AP_InertialSensor.h.
Referenced by AP_AHRS_DCM::drift_correction(), DataFlash_Class::Log_Write_IMU_instance(), run_test(), AP_AHRS_NavEKF::update_EKF2(), and AP_AHRS_NavEKF::update_EKF3().
|
inline |
Definition at line 141 of file AP_InertialSensor.h.
Referenced by calibrate_trim(), get_accel_health(), get_accel_health_all(), get_delta_velocity(), healthy(), and use_accel().
bool AP_InertialSensor::get_accel_health_all | ( | void | ) | const |
Definition at line 955 of file AP_InertialSensor.cpp.
Referenced by get_accel_health(), and AP_Arming::ins_checks().
|
inline |
Definition at line 152 of file AP_InertialSensor.h.
Referenced by display_offsets_and_scaling(), and GCS_MAVLINK::send_sensor_offsets().
|
inline |
Definition at line 153 of file AP_InertialSensor.h.
Referenced by get_accel_offsets().
|
inline |
Definition at line 249 of file AP_InertialSensor.h.
|
inline |
Definition at line 149 of file AP_InertialSensor.h.
Referenced by DataFlash_Class::Log_Write_IMU_instance().
|
inline |
Definition at line 156 of file AP_InertialSensor.h.
Referenced by display_offsets_and_scaling().
|
inline |
Definition at line 157 of file AP_InertialSensor.h.
Referenced by get_accel_scale().
|
inline |
Definition at line 242 of file AP_InertialSensor.h.
Referenced by AP_AK8963_BusDriver_Auxiliary::AP_AK8963_BusDriver_Auxiliary(), and get_auxiliary_bus().
AuxiliaryBus * AP_InertialSensor::get_auxiliary_bus | ( | int16_t | backend_id, |
uint8_t | instance | ||
) |
bool AP_InertialSensor::get_delta_angle | ( | uint8_t | i, |
Vector3f & | delta_angle | ||
) | const |
Definition at line 1445 of file AP_InertialSensor.cpp.
Referenced by get_gyro_offsets(), DataFlash_Class::Log_Write_IMUDT_instance(), AP_AHRS_DCM::matrix_update(), NavEKF2_core::readDeltaAngle(), and NavEKF3_core::readDeltaAngle().
|
inline |
Definition at line 108 of file AP_InertialSensor.h.
Referenced by get_delta_angle().
float AP_InertialSensor::get_delta_angle_dt | ( | uint8_t | i | ) | const |
Definition at line 1492 of file AP_InertialSensor.cpp.
Referenced by DataFlash_Class::Log_Write_IMUDT_instance(), NavEKF2_core::readDeltaAngle(), and NavEKF3_core::readIMUData().
|
inline |
Definition at line 111 of file AP_InertialSensor.h.
Referenced by get_delta_angle(), and get_delta_angle_dt().
|
inline |
Definition at line 174 of file AP_InertialSensor.h.
Referenced by get_delta_angle(), get_delta_angle_dt(), get_delta_velocity(), get_delta_velocity_dt(), DataFlash_Class::Log_Write_IMUDT_instance(), and AP_AHRS_DCM::update().
bool AP_InertialSensor::get_delta_velocity | ( | uint8_t | i, |
Vector3f & | delta_velocity | ||
) | const |
Definition at line 1462 of file AP_InertialSensor.cpp.
Referenced by AP_AHRS_DCM::drift_correction(), get_delta_angle_dt(), AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(), AP_AHRS::getCorrectedDeltaVelocityNED(), DataFlash_Class::Log_Write_IMUDT_instance(), NavEKF2_core::readDeltaVelocity(), and NavEKF3_core::readDeltaVelocity().
|
inline |
Definition at line 115 of file AP_InertialSensor.h.
Referenced by get_delta_velocity().
float AP_InertialSensor::get_delta_velocity_dt | ( | uint8_t | i | ) | const |
Definition at line 1477 of file AP_InertialSensor.cpp.
Referenced by AP_AHRS_DCM::drift_correction(), AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(), AP_AHRS::getCorrectedDeltaVelocityNED(), DataFlash_Class::Log_Write_IMUDT_instance(), NavEKF2_core::readDeltaVelocity(), and NavEKF3_core::readDeltaVelocity().
|
inline |
Definition at line 118 of file AP_InertialSensor.h.
Referenced by get_delta_velocity(), and get_delta_velocity_dt().
bool AP_InertialSensor::get_fixed_mount_accel_cal_sample | ( | uint8_t | sample_num, |
Vector3f & | ret | ||
) | const |
Definition at line 1774 of file AP_InertialSensor.cpp.
Referenced by _acal_save_calibrations(), and get_acal().
|
inline |
Fetch the current gyro values
Definition at line 99 of file AP_InertialSensor.h.
Referenced by NavEKF2_core::detectOptFlowTakeoff(), NavEKF3_core::detectOptFlowTakeoff(), AP_AHRS::get_gyro_latest(), AP_Arming::ins_checks(), DataFlash_Class::Log_Write_IMU_instance(), run_test(), GCS_MAVLINK::send_raw_imu(), AP_AHRS_NavEKF::update_EKF2(), and AP_AHRS_NavEKF::update_EKF3().
|
inline |
Definition at line 100 of file AP_InertialSensor.h.
Referenced by _init_gyro(), get_delta_angle(), and get_gyro().
|
inline |
Definition at line 134 of file AP_InertialSensor.h.
Referenced by _init_gyro(), _start_backends(), get_gyro_health_all(), gyro_calibrated_ok_all(), AP_Arming::ins_checks(), DataFlash_Class::Log_Write_IMU(), DataFlash_Class::Log_Write_IMUDT(), AP_AHRS_DCM::matrix_update(), NavEKF2_core::readDeltaAngle(), NavEKF3_core::readDeltaAngle(), run_test(), GCS_MAVLINK::send_raw_imu(), and setup().
|
inline |
Definition at line 178 of file AP_InertialSensor.h.
Referenced by AP_AHRS::AP_AHRS().
|
inline |
Definition at line 127 of file AP_InertialSensor.h.
Referenced by DataFlash_Class::Log_Write_IMU_instance().
|
inline |
Definition at line 210 of file AP_InertialSensor.h.
|
inline |
Definition at line 131 of file AP_InertialSensor.h.
Referenced by NavEKF2_core::detectOptFlowTakeoff(), NavEKF3_core::detectOptFlowTakeoff(), DataFlash_Class::Log_Write_IMU_instance(), AP_AHRS_DCM::matrix_update(), and run_test().
|
inline |
Definition at line 132 of file AP_InertialSensor.h.
Referenced by get_delta_angle(), get_gyro_health(), get_gyro_health_all(), healthy(), and use_gyro().
bool AP_InertialSensor::get_gyro_health_all | ( | void | ) | const |
Definition at line 916 of file AP_InertialSensor.cpp.
Referenced by get_gyro_health(), and AP_Arming::ins_checks().
|
inline |
Definition at line 103 of file AP_InertialSensor.h.
Referenced by display_offsets_and_scaling(), and GCS_MAVLINK::send_sensor_offsets().
|
inline |
Definition at line 104 of file AP_InertialSensor.h.
Referenced by get_gyro_offsets().
|
inline |
Definition at line 148 of file AP_InertialSensor.h.
Referenced by DataFlash_Class::Log_Write_IMU_instance().
|
inline |
Definition at line 160 of file AP_InertialSensor.h.
Referenced by AC_PrecLand::construct_pos_meas_using_rangefinder(), NavEKF2_core::readIMUData(), NavEKF3_core::readIMUData(), and AC_PrecLand::run_output_prediction().
|
inline |
Definition at line 163 of file AP_InertialSensor.h.
|
static |
Definition at line 479 of file AP_InertialSensor.cpp.
Referenced by AP::ins(), and AP_Compass_AK8963::probe_mpu9250().
|
inline |
Definition at line 276 of file AP_InertialSensor.h.
Referenced by NavEKF3::UpdateFilter(), and NavEKF2::UpdateFilter().
|
inline |
Definition at line 199 of file AP_InertialSensor.h.
Referenced by NavEKF2_core::InitialiseFilterBootstrap(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), NavEKF2_core::readIMUData(), and NavEKF3_core::readIMUData().
bool AP_InertialSensor::get_new_trim | ( | float & | trim_roll, |
float & | trim_pitch | ||
) |
Definition at line 1760 of file AP_InertialSensor.cpp.
Referenced by get_acal().
|
inline |
Definition at line 203 of file AP_InertialSensor.h.
Referenced by AP_AHRS_DCM::drift_correction(), AP_AHRS::get_primary_accel_index(), AP_AHRS_NavEKF::get_primary_accel_index(), AP_AHRS_NavEKF::get_primary_IMU_index(), NavEKF2_core::readIMUData(), NavEKF3_core::readIMUData(), AP_AHRS_NavEKF::update_EKF2(), and AP_AHRS_NavEKF::update_EKF3().
bool AP_InertialSensor::get_primary_accel_cal_sample_avg | ( | uint8_t | sample_num, |
Vector3f & | ret | ||
) | const |
Definition at line 1791 of file AP_InertialSensor.cpp.
Referenced by _acal_save_calibrations(), and get_acal().
|
inline |
Definition at line 204 of file AP_InertialSensor.h.
Referenced by AP_AHRS::get_primary_gyro_index(), AP_AHRS_NavEKF::get_primary_gyro_index(), NavEKF2_core::readIMUData(), and NavEKF3_core::readIMUData().
|
inline |
Definition at line 196 of file AP_InertialSensor.h.
Referenced by calibrate_trim(), NavEKF3::InitialiseFilter(), NavEKF2::InitialiseFilter(), and NavEKF3_core::setup_core().
|
inline |
Definition at line 169 of file AP_InertialSensor.h.
Referenced by DataFlash_Class::Log_Write_IMU_instance(), and run_test().
|
inline |
Definition at line 222 of file AP_InertialSensor.h.
Referenced by get_vibration_levels(), is_still(), DataFlash_Class::Log_Write_Vibration(), and GCS_MAVLINK::send_vibration().
Vector3f AP_InertialSensor::get_vibration_levels | ( | uint8_t | instance | ) | const |
|
inline |
Definition at line 135 of file AP_InertialSensor.h.
Referenced by gyro_calibrated_ok_all().
bool AP_InertialSensor::gyro_calibrated_ok_all | ( | ) | const |
Definition at line 928 of file AP_InertialSensor.cpp.
Referenced by gyro_calibrated_ok(), and AP_Arming::ins_checks().
|
inline |
Definition at line 138 of file AP_InertialSensor.h.
Referenced by init().
|
inline |
Definition at line 201 of file AP_InertialSensor.h.
Referenced by AP_AHRS_DCM::drift_correction().
void AP_InertialSensor::init | ( | uint16_t | sample_rate_hz | ) |
Perform startup initialisation.
Called to initialise the state of the IMU.
Gyros will be calibrated unless INS_GYRO_CAL is zero
style | The initialisation startup style. |
Definition at line 609 of file AP_InertialSensor.cpp.
Referenced by setup(), and SchedTest::setup().
void AP_InertialSensor::init_gyro | ( | void | ) |
Perform cold-start initialisation for just the gyros.
Definition at line 898 of file AP_InertialSensor.cpp.
Referenced by GCS_MAVLINK::calibrate_gyros(), calibrating(), and init().
bool AP_InertialSensor::is_still | ( | ) |
Definition at line 1647 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels(), and AP_TempCalibration::learn_calibration().
|
delete |
void AP_InertialSensor::periodic | ( | ) |
uint8_t AP_InertialSensor::register_accel | ( | uint16_t | raw_sample_rate_hz, |
uint32_t | id | ||
) |
Definition at line 524 of file AP_InertialSensor.cpp.
Referenced by AP_InertialSensor_HIL::_init_sensor(), AP_InertialSensor_PX4::_init_sensor(), AP_InertialSensor_SITL::init_sensor(), AP_InertialSensor_LSM9DS0::start(), AP_InertialSensor_LSM9DS1::start(), AP_InertialSensor_L3G4200D::start(), AP_InertialSensor_BMI160::start(), AP_InertialSensor_RST::start(), and AP_InertialSensor_Invensense::start().
uint8_t AP_InertialSensor::register_gyro | ( | uint16_t | raw_sample_rate_hz, |
uint32_t | id | ||
) |
Register a new gyro/accel driver, allocating an instance number
Definition at line 490 of file AP_InertialSensor.cpp.
Referenced by AP_InertialSensor_HIL::_init_sensor(), AP_InertialSensor_PX4::_init_sensor(), AP_InertialSensor_SITL::init_sensor(), AP_InertialSensor_LSM9DS0::start(), AP_InertialSensor_LSM9DS1::start(), AP_InertialSensor_L3G4200D::start(), AP_InertialSensor_BMI160::start(), AP_InertialSensor_RST::start(), and AP_InertialSensor_Invensense::start().
void AP_InertialSensor::set_accel | ( | uint8_t | instance, |
const Vector3f & | accel | ||
) |
Definition at line 1508 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels().
void AP_InertialSensor::set_accel_peak_hold | ( | uint8_t | instance, |
const Vector3f & | accel | ||
) |
Definition at line 1617 of file AP_InertialSensor.cpp.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and get_auxiliary_bus().
|
inline |
Definition at line 190 of file AP_InertialSensor.h.
Referenced by AP_AHRS::set_orientation().
void AP_InertialSensor::set_delta_angle | ( | uint8_t | instance, |
const Vector3f & | deltaa, | ||
float | deltaat | ||
) |
Definition at line 1568 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels().
void AP_InertialSensor::set_delta_time | ( | float | delta_time | ) |
Definition at line 1548 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels().
void AP_InertialSensor::set_delta_velocity | ( | uint8_t | instance, |
float | deltavt, | ||
const Vector3f & | deltav | ||
) |
Definition at line 1556 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels().
void AP_InertialSensor::set_gyro | ( | uint8_t | instance, |
const Vector3f & | gyro | ||
) |
Definition at line 1526 of file AP_InertialSensor.cpp.
Referenced by get_vibration_levels().
|
inline |
Definition at line 207 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_HIL::_init_sensor().
|
inline |
MAV_RESULT AP_InertialSensor::simple_accel_cal | ( | ) |
Definition at line 1820 of file AP_InertialSensor.cpp.
Referenced by GCS_MAVLINK::_handle_command_preflight_calibration(), and get_acal().
void AP_InertialSensor::update | ( | void | ) |
Definition at line 1233 of file AP_InertialSensor.cpp.
Referenced by _init_gyro(), calibrate_trim(), get_gyro_drift_rate(), SchedTest::ins_update(), AP_AHRS_DCM::reset(), run_test(), simple_accel_cal(), and AP_AHRS_DCM::update().
bool AP_InertialSensor::use_accel | ( | uint8_t | instance | ) | const |
Definition at line 1068 of file AP_InertialSensor.cpp.
Referenced by AP_AHRS_DCM::drift_correction(), get_accel_count(), AP_Arming::ins_checks(), NavEKF2_core::readIMUData(), and NavEKF3_core::readIMUData().
bool AP_InertialSensor::use_gyro | ( | uint8_t | instance | ) | const |
Definition at line 945 of file AP_InertialSensor.cpp.
Referenced by gyro_calibrated_ok(), AP_Arming::ins_checks(), DataFlash_Class::Log_Write_IMUDT(), NavEKF2_core::readIMUData(), and NavEKF3_core::readIMUData().
void AP_InertialSensor::wait_for_sample | ( | void | ) |
Definition at line 1343 of file AP_InertialSensor.cpp.
Referenced by calibrate_trim(), get_gyro_drift_rate(), AP_Scheduler::loop(), AP_AHRS_DCM::reset(), run_test(), and update().
|
friend |
Definition at line 49 of file AP_InertialSensor.h.
|
private |
Definition at line 560 of file AP_InertialSensor.h.
Referenced by acal_init(), acal_update(), and get_acal().
|
private |
Definition at line 556 of file AP_InertialSensor.h.
Referenced by get_fixed_mount_accel_cal_sample().
|
private |
Definition at line 388 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_accel(), get_accel(), and set_accel().
|
private |
Definition at line 575 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), and accel_cal_requires_reboot().
|
private |
Definition at line 562 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), AP_InertialSensor_Backend::_publish_accel(), acal_init(), get_fixed_mount_accel_cal_sample(), and get_primary_accel_cal_sample_avg().
|
private |
Definition at line 535 of file AP_InertialSensor.h.
Referenced by calc_vibration_and_clipping(), get_accel_clip_count(), and AP_InertialSensor_Backend::increment_clip_count().
|
private |
Definition at line 379 of file AP_InertialSensor.h.
Referenced by _acal_event_failure(), _acal_save_calibrations(), _start_backends(), get_accel_count(), get_accel_health(), get_fixed_mount_accel_cal_sample(), get_primary_accel_cal_sample_avg(), init(), register_accel(), AP_InertialSensor::BatchSampler::rotate_to_next_sensor(), and set_accel().
|
private |
Definition at line 531 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_inc_accel_error_count(), AP_InertialSensor_Backend::_set_accel_error_count(), get_accel_error_count(), and update().
|
private |
Definition at line 398 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and AP_InertialSensor_Backend::update_accel().
|
private |
Definition at line 469 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_accel_filter_cutoff(), and get_accel_filter_hz().
|
private |
Definition at line 400 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and AP_InertialSensor_Backend::update_accel().
|
private |
Definition at line 529 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_accel(), get_accel_health(), set_accel(), set_gyro(), and update().
|
private |
Definition at line 432 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), _start_backends(), accel_calibrated_ok_all(), register_accel(), and simple_accel_cal().
|
private |
Definition at line 492 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), accel_calibrated_ok_all(), register_accel(), and simple_accel_cal().
|
private |
Definition at line 456 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample().
|
private |
Definition at line 444 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_accel_max_abs_offset(), and AP_InertialSensor().
|
private |
Definition at line 437 of file AP_InertialSensor.h.
Referenced by _acal_event_failure(), _acal_save_calibrations(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), accel_calibrated_ok_all(), get_accel_offsets(), and simple_accel_cal().
|
private |
Definition at line 488 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_rotate_and_correct_accel(), and AP_InertialSensor_Backend::set_accel_orientation().
|
private |
Definition at line 451 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_accel_oversampling(), get_accel_rate_hz(), AP_InertialSensor::BatchSampler::push_data_to_log(), and register_accel().
|
private |
Definition at line 441 of file AP_InertialSensor.h.
Referenced by get_imu_pos_offset().
|
private |
Definition at line 447 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_accel_raw_sample_rate(), AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::_set_accel_raw_sample_rate(), get_accel_rate_hz(), AP_InertialSensor::BatchSampler::push_data_to_log(), and register_accel().
|
private |
Definition at line 424 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_raw_sample_accel_multiplier(), register_accel(), and AP_InertialSensor::BatchSampler::rotate_to_next_sensor().
|
private |
Definition at line 436 of file AP_InertialSensor.h.
Referenced by _acal_event_failure(), _acal_save_calibrations(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), accel_calibrated_ok_all(), get_accel_scale(), init(), and simple_accel_cal().
|
private |
Definition at line 420 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_accel_sensor_rate_sampling_enabled(), and AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging().
|
private |
Definition at line 578 of file AP_InertialSensor.h.
Referenced by update().
|
private |
Definition at line 537 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor(), calc_vibration_and_clipping(), and get_vibration_levels().
|
private |
Definition at line 536 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor(), and calc_vibration_and_clipping().
|
private |
Definition at line 380 of file AP_InertialSensor.h.
Referenced by _add_backend(), _find_backend(), _start_backends(), detect_backends(), update(), and wait_for_sample().
|
private |
Definition at line 373 of file AP_InertialSensor.h.
Referenced by _add_backend(), _find_backend(), _start_backends(), update(), and wait_for_sample().
|
private |
Definition at line 510 of file AP_InertialSensor.h.
Referenced by _find_backend(), and detect_backends().
|
private |
Definition at line 483 of file AP_InertialSensor.h.
Referenced by _init_gyro(), AP_InertialSensor_Backend::_publish_accel(), AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_InertialSensor_Backend::_rotate_and_correct_gyro(), get_fixed_mount_accel_cal_sample(), get_primary_accel_cal_sample_avg(), set_board_orientation(), and simple_accel_cal().
|
private |
Definition at line 508 of file AP_InertialSensor.h.
Referenced by _init_gyro(), calibrate_trim(), calibrating(), and simple_accel_cal().
|
private |
Definition at line 484 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_rotate_and_correct_accel(), AP_InertialSensor_Backend::_rotate_and_correct_gyro(), get_fixed_mount_accel_cal_sample(), get_primary_accel_cal_sample_avg(), and set_board_orientation().
|
private |
Definition at line 410 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_gyro(), get_delta_angle(), and set_delta_angle().
|
private |
Definition at line 415 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_InertialSensor_Backend::_publish_gyro(), and update().
|
private |
Definition at line 414 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_InertialSensor_Backend::_publish_gyro(), and update().
|
private |
Definition at line 411 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_gyro(), get_delta_angle_dt(), and set_delta_angle().
|
private |
Definition at line 412 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_gyro(), get_delta_angle(), get_delta_angle_dt(), set_delta_angle(), and update().
|
private |
Definition at line 513 of file AP_InertialSensor.h.
Referenced by get_delta_time(), init(), and wait_for_sample().
|
private |
Definition at line 389 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_accel(), get_delta_velocity(), and set_delta_velocity().
|
private |
Definition at line 393 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::_publish_accel(), and update().
|
private |
Definition at line 395 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::_publish_accel(), and update().
|
private |
Definition at line 390 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_accel(), get_delta_velocity_dt(), and set_delta_velocity().
|
private |
Definition at line 391 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_accel(), get_delta_velocity(), get_delta_velocity_dt(), set_delta_velocity(), and update().
|
private |
Definition at line 480 of file AP_InertialSensor.h.
Referenced by detect_backends().
|
private |
Definition at line 477 of file AP_InertialSensor.h.
Referenced by detect_backends(), and AP_InertialSensor_Backend::enable_fast_sampling().
|
private |
Definition at line 409 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_gyro(), get_gyro(), set_gyro(), and update().
|
private |
Definition at line 491 of file AP_InertialSensor.h.
Referenced by _init_gyro(), AP_InertialSensor(), gyro_calibrated_ok(), register_gyro(), and set_gyro().
|
private |
Definition at line 471 of file AP_InertialSensor.h.
Referenced by gyro_calibration_timing().
|
private |
Definition at line 378 of file AP_InertialSensor.h.
Referenced by _save_gyro_calibration(), _start_backends(), get_gyro_count(), get_gyro_health(), init(), register_gyro(), AP_InertialSensor::BatchSampler::rotate_to_next_sensor(), and set_gyro().
|
private |
Definition at line 532 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_inc_gyro_error_count(), AP_InertialSensor_Backend::_set_gyro_error_count(), get_gyro_error_count(), and update().
|
private |
Definition at line 399 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), and AP_InertialSensor_Backend::update_gyro().
|
private |
Definition at line 470 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_gyro_filter_cutoff(), and get_gyro_filter_hz().
|
private |
Definition at line 401 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), and AP_InertialSensor_Backend::update_gyro().
|
private |
Definition at line 528 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_gyro(), get_gyro_health(), set_gyro(), and update().
|
private |
Definition at line 433 of file AP_InertialSensor.h.
Referenced by _save_gyro_calibration(), _start_backends(), gyro_calibrated_ok_all(), and register_gyro().
|
private |
Definition at line 457 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample().
|
private |
Definition at line 438 of file AP_InertialSensor.h.
Referenced by _init_gyro(), AP_InertialSensor_Backend::_rotate_and_correct_gyro(), _save_gyro_calibration(), and get_gyro_offsets().
|
private |
Definition at line 487 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_rotate_and_correct_gyro(), and AP_InertialSensor_Backend::set_gyro_orientation().
|
private |
Definition at line 452 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_gyro_oversampling(), get_gyro_rate_hz(), AP_InertialSensor::BatchSampler::push_data_to_log(), and register_gyro().
|
private |
Definition at line 448 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_gyro_raw_sample_rate(), AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_InertialSensor_Backend::_set_gyro_raw_sample_rate(), get_gyro_rate_hz(), AP_InertialSensor::BatchSampler::push_data_to_log(), and register_gyro().
|
private |
Definition at line 425 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_raw_sampl_gyro_multiplier(), register_gyro(), and AP_InertialSensor::BatchSampler::rotate_to_next_sensor().
|
private |
Definition at line 421 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_set_gyro_sensor_rate_sampling_enabled(), and AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging().
|
private |
Definition at line 579 of file AP_InertialSensor.h.
Referenced by update().
|
private |
Definition at line 502 of file AP_InertialSensor.h.
Referenced by init(), update(), and wait_for_sample().
struct { ... } AP_InertialSensor::_hil |
Referenced by set_delta_time(), and wait_for_sample().
|
private |
Definition at line 505 of file AP_InertialSensor.h.
Referenced by accel_calibrated_ok_all(), detect_backends(), set_hil_mode(), update(), and wait_for_sample().
|
private |
Definition at line 416 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample().
|
private |
Definition at line 417 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample().
|
private |
Definition at line 516 of file AP_InertialSensor.h.
Referenced by init(), and wait_for_sample().
|
private |
Definition at line 525 of file AP_InertialSensor.h.
Referenced by get_last_update_usec(), and update().
|
private |
Definition at line 499 of file AP_InertialSensor.h.
Referenced by set_log_raw_bit(), and AP_InertialSensor_Backend::should_log_imu_raw().
|
private |
Definition at line 384 of file AP_InertialSensor.h.
Referenced by get_loop_delta_t(), and init().
|
private |
Definition at line 385 of file AP_InertialSensor.h.
Referenced by get_delta_angle_dt(), get_delta_time(), get_delta_velocity_dt(), and init().
|
private |
Definition at line 402 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), AP_InertialSensor_Backend::update_accel(), and wait_for_sample().
|
private |
Definition at line 403 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), AP_InertialSensor_Backend::update_gyro(), and wait_for_sample().
|
private |
Definition at line 573 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), and get_new_trim().
|
private |
Definition at line 519 of file AP_InertialSensor.h.
Referenced by init(), and wait_for_sample().
|
private |
Definition at line 406 of file AP_InertialSensor.h.
|
private |
Definition at line 428 of file AP_InertialSensor.h.
|
private |
Referenced by get_accel_peak_hold_neg_x(), and set_accel_peak_hold().
|
private |
Definition at line 496 of file AP_InertialSensor.h.
Referenced by get_accel(), get_accel_health(), get_accel_offsets(), get_accel_scale(), get_delta_angle_dt(), get_delta_velocity(), get_delta_velocity_dt(), get_imu_pos_offset(), get_primary_accel(), get_vibration_levels(), set_accel(), set_accel_peak_hold(), set_gyro(), and update().
|
private |
Definition at line 495 of file AP_InertialSensor.h.
Referenced by get_delta_angle(), get_gyro(), get_gyro_health(), get_gyro_offsets(), get_primary_gyro(), and update().
|
staticprivate |
Definition at line 559 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor(), and get_instance().
|
private |
Definition at line 460 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and AP_InertialSensor_Backend::notify_accel_fifo_reset().
|
private |
Definition at line 461 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_raw_sample(), and AP_InertialSensor_Backend::notify_accel_fifo_reset().
|
private |
Definition at line 462 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), and AP_InertialSensor_Backend::notify_gyro_fifo_reset().
|
private |
Definition at line 463 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(), and AP_InertialSensor_Backend::notify_gyro_fifo_reset().
|
private |
Definition at line 522 of file AP_InertialSensor.h.
Referenced by init(), and wait_for_sample().
|
private |
Definition at line 383 of file AP_InertialSensor.h.
Referenced by get_sample_rate(), AP_InertialSensor_Backend::get_sample_rate_hz(), and init().
|
private |
Definition at line 580 of file AP_InertialSensor.h.
Referenced by update().
|
private |
Definition at line 581 of file AP_InertialSensor.h.
Referenced by update().
|
private |
Definition at line 546 of file AP_InertialSensor.h.
Referenced by is_still().
|
private |
Definition at line 466 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_publish_temperature(), and get_temperature().
|
private |
Definition at line 557 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations().
|
private |
Definition at line 571 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), and get_new_trim().
|
private |
Definition at line 572 of file AP_InertialSensor.h.
Referenced by _acal_save_calibrations(), and get_new_trim().
|
private |
Definition at line 474 of file AP_InertialSensor.h.
Referenced by update(), use_accel(), and use_gyro().
BatchSampler AP_InertialSensor::batchsampler {*this} |
Definition at line 352 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(), AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(), init(), AP_InertialSensor_Backend::log_accel_raw(), AP_InertialSensor_Backend::log_gyro_raw(), and periodic().
float AP_InertialSensor::delta_time |
Definition at line 552 of file AP_InertialSensor.h.
Referenced by get_vibration_levels(), and set_delta_time().
|
static |
Definition at line 187 of file AP_InertialSensor.h.
Referenced by AP_InertialSensor().