// 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();
}
