Line data    Source code

       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      908911 :     uint16_t get_loop_rate_hz(void) const { return _RISH.loop_rate_hz; }
      14             : 
      15    83454883 :     const Vector3f &get_imu_pos_offset(uint8_t instance) const {
      16    83454883 :         return pos[instance];
      17             :     }
      18             : 
      19             :     // accel stuff
      20    83457389 :     uint8_t get_accel_count(void) const { return _RISH.accel_count; }
      21           0 :     uint8_t get_first_usable_accel(void) const { return _RISH.first_usable_accel; };
      22             : 
      23   333819532 :     bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }
      24   192215716 :     const Vector3f     &get_accel(uint8_t i) const { return accel_filtered[i]; }
      25    83454883 :     bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const {
      26    83454883 :         delta_velocity = _RISI[i].delta_velocity;
      27    83454883 :         delta_velocity_dt = _RISI[i].delta_velocity_dt;
      28    83454883 :         return _RISI[i].get_delta_velocity_ret;
      29             :     }
      30             : 
      31             :     // gyro stuff
      32    83454883 :     uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }
      33           0 :     uint8_t get_first_usable_gyro(void) const { return _RISH.first_usable_gyro; };
      34             : 
      35   333819532 :     bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }
      36    19965274 :     float get_gyro_bias_limit(uint8_t instance) const { return _RISJ[instance].gyro_bias_limit; }
      37        5866 :     float get_gyro_bias_init_dps(uint8_t instance) const { return _RISJ[instance].gyro_bias_init_dps; }
      38   192340828 :     const Vector3f     &get_gyro(uint8_t i) const { return gyro_filtered[i]; }
      39             :     const Vector3f     &get_gyro() const { return get_gyro(_primary_gyro); }
      40    83454883 :     bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const {
      41    83454883 :         delta_angle = _RISI[i].delta_angle;
      42    83454883 :         delta_angle_dt = _RISI[i].delta_angle_dt;
      43    83454883 :         return _RISI[i].get_delta_angle_ret;
      44             :     }
      45             : 
      46             :     // return the main loop delta_t in seconds
      47    83464293 :     float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }
      48             : 
      49             :     // AP_DAL methods:
      50             :     AP_DAL_InertialSensor();
      51             : 
      52             :     void start_frame();
      53             : 
      54             :     void handle_message(const log_RISH &msg) {
      55             :         _RISH = msg;
      56             :     }
      57             :     void handle_message(const log_RISI &msg) {
      58             :         _RISI[msg.instance] = msg;
      59             :         pos[msg.instance] = AP::ins().get_imu_pos_offset(msg.instance);
      60             :         update_filtered(msg.instance);
      61             :     }
      62             :     void handle_message(const log_RISJ &msg) {
      63             :         _RISJ[msg.instance] = msg;
      64             :     }
      65             : 
      66             : private:
      67             :     struct log_RISH _RISH;
      68             :     struct log_RISI _RISI[INS_MAX_INSTANCES];
      69             :     struct log_RISJ _RISJ[INS_MAX_INSTANCES];
      70             :     float alpha;
      71             : 
      72             :     // sensor positions
      73             :     Vector3f pos[INS_MAX_INSTANCES];
      74             : 
      75             :     Vector3f gyro_filtered[INS_MAX_INSTANCES];
      76             :     Vector3f accel_filtered[INS_MAX_INSTANCES];
      77             : 
      78             :     uint8_t _primary_gyro;
      79             : 
      80             :     void update_filtered(uint8_t i);
      81             : };