CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_DAL/AP_DAL_InertialSensor.cpp
Views: 1798
1
#include "AP_DAL_InertialSensor.h"

2


3
#include <AP_Logger/AP_Logger.h>

4
#include "AP_DAL.h"

5


6
AP_DAL_InertialSensor::AP_DAL_InertialSensor()

7
{

8
    for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {

9
        _RISI[i].instance = i;

10
    }

11


12
}

13


14
void AP_DAL_InertialSensor::start_frame()

15
{

16
    const auto &ins = AP::ins();

17


18
    const log_RISH old_RISH = _RISH;

19


20
    _RISH.loop_rate_hz = ins.get_loop_rate_hz();

21
    _RISH.first_usable_gyro = ins.get_first_usable_gyro();

22
    _RISH.loop_delta_t = ins.get_loop_delta_t();

23
    _RISH.first_usable_accel = ins.get_first_usable_accel();

24
    _RISH.accel_count = ins.get_accel_count();

25
    _RISH.gyro_count = ins.get_gyro_count();

26
    WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);

27


28
    for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {

29
        log_RISI &RISI = _RISI[i];

30
        const log_RISI old_RISI = RISI;

31


32
        // accel data

33
        RISI.use_accel = ins.use_accel(i);

34
        if (RISI.use_accel) {

35
            RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt);

36
        }

37


38
        // gryo data

39
        RISI.use_gyro = ins.use_gyro(i);

40
        if (RISI.use_gyro) {

41
            RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);

42
        }

43


44
        update_filtered(i);

45


46
        WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);

47


48
        // update sensor position

49
        pos[i] = ins.get_imu_pos_offset(i);

50
    }

51
}

52


53
// update filtered gyro and accel

54
void AP_DAL_InertialSensor::update_filtered(uint8_t i)

55
{

56
    if (!is_positive(alpha)) {

57
        // we use a constant 10Hz for EKF filtered accel/gyro, making the EKF

58
        // independent of the INS filter settings

59
        const float cutoff_hz = 10.0;

60
        alpha = calc_lowpass_alpha_dt(get_loop_delta_t(), cutoff_hz);

61
    }

62
    if (is_positive(_RISI[i].delta_angle_dt)) {

63
        gyro_filtered[i] += ((_RISI[i].delta_angle/_RISI[i].delta_angle_dt) - gyro_filtered[i]) * alpha;

64
    }

65
    if (is_positive(_RISI[i].delta_velocity_dt)) {

66
        accel_filtered[i] += ((_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) - accel_filtered[i]) * alpha;

67
    }

68
}

69


70