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.h
Views: 1798
1
#pragma once

2


3
#include <AP_InertialSensor/AP_InertialSensor.h>

4


5
#include <AP_Logger/LogStructure.h>

6


7
class AP_DAL_InertialSensor {

8
public:

9


10
    // InertialSensor-like methods:

11


12
    // return the selected loop rate at which samples are made available

13
    uint16_t get_loop_rate_hz(void) const { return _RISH.loop_rate_hz; }

14


15
    const Vector3f &get_imu_pos_offset(uint8_t instance) const {

16
        return pos[instance];

17
    }

18


19
    // accel stuff

20
    uint8_t get_accel_count(void) const { return _RISH.accel_count; }

21
    uint8_t get_first_usable_accel(void) const { return _RISH.first_usable_accel; };

22


23
    bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }

24
    const Vector3f     &get_accel(uint8_t i) const { return accel_filtered[i]; }

25
    bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const {

26
        delta_velocity = _RISI[i].delta_velocity;

27
        delta_velocity_dt = _RISI[i].delta_velocity_dt;

28
        return _RISI[i].get_delta_velocity_ret;

29
    }

30


31
    // gyro stuff

32
    uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }

33
    uint8_t get_first_usable_gyro(void) const { return _RISH.first_usable_gyro; };

34


35
    bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }

36
    const Vector3f     &get_gyro(uint8_t i) const { return gyro_filtered[i]; }

37
    const Vector3f     &get_gyro() const { return get_gyro(_primary_gyro); }

38
    bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const {

39
        delta_angle = _RISI[i].delta_angle;

40
        delta_angle_dt = _RISI[i].delta_angle_dt;

41
        return _RISI[i].get_delta_angle_ret;

42
    }

43


44
    // return the main loop delta_t in seconds

45
    float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }

46


47
    // AP_DAL methods:

48
    AP_DAL_InertialSensor();

49


50
    void start_frame();

51


52
    void handle_message(const log_RISH &msg) {

53
        _RISH = msg;

54
    }

55
    void handle_message(const log_RISI &msg) {

56
        _RISI[msg.instance] = msg;

57
        pos[msg.instance] = AP::ins().get_imu_pos_offset(msg.instance);

58
        update_filtered(msg.instance);

59
    }

60


61
private:

62
    struct log_RISH _RISH;

63
    struct log_RISI _RISI[INS_MAX_INSTANCES];

64
    float alpha;

65


66
    // sensor positions

67
    Vector3f pos[INS_MAX_INSTANCES];

68


69
    Vector3f gyro_filtered[INS_MAX_INSTANCES];

70
    Vector3f accel_filtered[INS_MAX_INSTANCES];

71


72
    uint8_t _primary_gyro;

73


74
    void update_filtered(uint8_t i);

75
};

76


77