mirror of https://github.com/ArduPilot/ardupilot
69 lines
2.1 KiB
C++
69 lines
2.1 KiB
C++
#include "AP_DAL_InertialSensor.h"
|
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include "AP_DAL.h"
|
|
|
|
AP_DAL_InertialSensor::AP_DAL_InertialSensor()
|
|
{
|
|
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
|
|
_RISI[i].instance = i;
|
|
}
|
|
|
|
}
|
|
|
|
void AP_DAL_InertialSensor::start_frame()
|
|
{
|
|
const auto &ins = AP::ins();
|
|
|
|
const log_RISH old_RISH = _RISH;
|
|
|
|
_RISH.loop_rate_hz = ins.get_loop_rate_hz();
|
|
_RISH.primary_gyro = ins.get_primary_gyro();
|
|
_RISH.loop_delta_t = ins.get_loop_delta_t();
|
|
_RISH.primary_accel = ins.get_primary_accel();
|
|
_RISH.accel_count = ins.get_accel_count();
|
|
_RISH.gyro_count = ins.get_gyro_count();
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
|
|
log_RISI &RISI = _RISI[i];
|
|
const log_RISI old_RISI = RISI;
|
|
|
|
// accel data
|
|
RISI.use_accel = ins.use_accel(i);
|
|
if (RISI.use_accel) {
|
|
RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt);
|
|
}
|
|
|
|
// gryo data
|
|
RISI.use_gyro = ins.use_gyro(i);
|
|
if (RISI.use_gyro) {
|
|
RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);
|
|
}
|
|
|
|
update_filtered(i);
|
|
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);
|
|
|
|
// update sensor position
|
|
pos[i] = ins.get_imu_pos_offset(i);
|
|
}
|
|
}
|
|
|
|
// update filtered gyro and accel
|
|
void AP_DAL_InertialSensor::update_filtered(uint8_t i)
|
|
{
|
|
if (!is_positive(alpha)) {
|
|
// we use a constant 20Hz for EKF filtered accel/gyro, making the EKF
|
|
// independent of the INS filter settings
|
|
const float cutoff_hz = 20.0;
|
|
alpha = calc_lowpass_alpha_dt(get_loop_delta_t(), cutoff_hz);
|
|
}
|
|
if (is_positive(_RISI[i].delta_angle_dt)) {
|
|
gyro_filtered[i] += ((_RISI[i].delta_angle/_RISI[i].delta_angle_dt) - gyro_filtered[i]) * alpha;
|
|
}
|
|
if (is_positive(_RISI[i].delta_velocity_dt)) {
|
|
accel_filtered[i] += ((_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) - accel_filtered[i]) * alpha;
|
|
}
|
|
}
|