// run a full MPU6000 update round void AP_AHRS_MPU6000::update(void) { float delta_t; // tell the IMU to grab some data. if( !_secondary_ahrs ) { _ins->update(); } // ask the IMU how much time this sensor reading represents delta_t = _ins->get_delta_time(); // convert the quaternions into a DCM matrix _mpu6000->quaternion.rotation_matrix(_dcm_matrix); // we run the gyro bias correction using gravity vector algorithm drift_correction(delta_t); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // prepare earth frame accelerometer values for ArduCopter Inertial Navigation and accel-based throttle _accel_ef = _dcm_matrix * _ins->get_accel(); }
// run a full DCM update round void AP_AHRS_DCM::update(void) { float delta_t; // tell the IMU to grab some data _imu->update(); // ask the IMU how much time this sensor reading represents delta_t = _imu->get_delta_time(); // Get current values for gyros _gyro_vector = _imu->get_gyro(); _accel_vector = _imu->get_accel(); // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); }
// run a full DCM update round void AP_AHRS_DCM::update(void) { float delta_t; // tell the IMU to grab some data _ins.update(); // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { _ra_sum.zero(); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); }
void AP_DCM::update_DCM(void) { read_adc_raw(); // Get current values for IMU sensors matrix_update(); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation }
void AP_DCM_FW::update_DCM(float _G_Dt) { _gyro_vector = _imu.get_gyro(); // Get current values for IMU sensors _accel_vector = _imu.get_accel(); // Get current values for IMU sensors matrix_update(_G_Dt); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation }
// run a full DCM update round void AP_AHRS_DCM::update(bool skip_ins_update) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); float delta_t; if (_last_startup_ms == 0) { _last_startup_ms = AP_HAL::millis(); load_watchdog_home(); } if (!skip_ins_update) { // tell the IMU to grab some data AP::ins().update(); } const AP_InertialSensor &_ins = AP::ins(); // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // update trig values including _cos_roll, cos_pitch update_trig(); // update AOA and SSA update_AOA_SSA(); backup_attitude(); }
void AP_DCM::update_DCM(void) { float delta_t; _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors delta_t = _imu->get_delta_time(); matrix_update(delta_t); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation }
void AP_DCM::update_DCM_fast(void) { float delta_t; _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors delta_t = _imu->get_delta_time(); matrix_update(delta_t); // Integrate the DCM matrix switch(_toggle++){ case 0: normalize(); // Normalize the DCM matrix break; case 1: //drift_correction(); // Normalize the DCM matrix euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation break; case 2: drift_correction(); // Normalize the DCM matrix break; case 3: //drift_correction(); // Normalize the DCM matrix euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation break; case 4: euler_yaw(); break; default: euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation _toggle = 0; //drift_correction(); // Normalize the DCM matrix break; } }
// run a full DCM update round void BC_AHRS::update(void) { float delta_t; // GYRO+ACCの取得 _ins.get_data(); // GYRO+ACCの取得にかかった時間を取得 delta_t = _ins.get_delta_time(); // 取得時間が0.2sec以上だったら捨てる // CopterにおいてArm時等にそうなる if (delta_t > 0.2f) { memset(&_ra_sum, 0, sizeof(_ra_sum)); // _ra_sumを0で埋めてる _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs // (超訳)ジャイロ値で方向余弦行列を更新 // ★チェックOK matrix_update(delta_t); // Normalize the DCM matrix // (超訳)方向余弦行列をノーマライズ // ★チェックOK normalize(); // Perform drift correction // (超訳)ドリフト補正を実施 drift_correction(delta_t); // paranoid check for bad values in the DCM matrix // (超訳)方向余弦行列中の不正値をチェック check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation // (超訳)ロール、ピッチ、ヨーを計算 euler_angles(); // update trig values including _cos_roll, cos_pitch // (超訳)必要な値(ex. rollのcos値,等)を計算 update_trig(); }
// run a full DCM update round void AP_AHRS_DCM::update(int16_t attitude[3], float delta_t) { // Get current values for gyros _gyro_vector = gyro_attitude; _accel_vector = accel; // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction //setCurrentProfiledActivity(DRIFT_CORRECTION); drift_correction(delta_t); // paranoid check for bad values in the DCM matrix //setCurrentProfiledActivity(CHECK_MATRIX); check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation //setCurrentProfiledActivity(EULER_ANGLES); euler_angles(); //setCurrentProfiledActivity(ANGLESOUTPUT); attitude[0] = roll * INT16DEG_PI_FACTOR; attitude[1] = pitch* INT16DEG_PI_FACTOR; attitude[2] = yaw * INT16DEG_PI_FACTOR; // Just for info: int16_t sensor = degrees(roll) * 10; debugOut.analog[0] = sensor; sensor = degrees(pitch) * 10; debugOut.analog[1] = sensor; sensor = degrees(yaw) * 10; if (sensor < 0) sensor += 3600; debugOut.analog[2] = sensor; }
// run a full MPU6000 update round void AP_AHRS_MPU6000::update(void) { float delta_t; // tell the IMU to grab some data. is this necessary? _imu->update(); // ask the IMU how much time this sensor reading represents delta_t = _imu->get_delta_time(); // convert the quaternions into a DCM matrix _mpu6000->quaternion.rotation_matrix(_dcm_matrix); // we run the gyro bias correction using gravity vector algorithm drift_correction(delta_t); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); }