ardupilot/libraries/AP_DAL/AP_DAL_InertialSensor.cpp

69 lines
2.1 KiB
C++
Raw Normal View History

#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();
2020-11-07 17:29:45 -04:00
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;
2020-11-08 05:11:58 -04:00
// accel data
RISI.use_accel = ins.use_accel(i);
2020-11-08 05:11:58 -04:00
if (RISI.use_accel) {
RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt);
2020-11-08 05:11:58 -04:00
}
2020-11-08 05:11:58 -04:00
// gryo data
RISI.use_gyro = ins.use_gyro(i);
2020-11-08 05:11:58 -04:00
if (RISI.use_gyro) {
RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);
2020-11-08 05:11:58 -04:00
}
update_filtered(i);
2020-11-07 17:29:45 -04:00
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;
}
}