Line data    Source code

       1             : #include "AP_DAL_InertialSensor.h"
       2             : 
       3             : #include <AP_Logger/AP_Logger.h>
       4             : #include "AP_DAL.h"
       5             : 
       6        8772 : AP_DAL_InertialSensor::AP_DAL_InertialSensor()
       7             : {
       8        8772 :     for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
       9        6579 :         _RISI[i].instance = i;
      10             :     }
      11             :     // seed RISJ with legacy defaults so replay of older logs (with no
      12             :     // RISJ messages) still gives the EKF sane gyro bias clamp/init values.
      13        8772 :     for (uint8_t i=0; i<ARRAY_SIZE(_RISJ); i++) {
      14        6579 :         _RISJ[i].instance = i;
      15        6579 :         _RISJ[i].gyro_bias_limit = 0.5f;
      16        6579 :         _RISJ[i].gyro_bias_init_dps = 2.5f;
      17             :     }
      18        2193 : }
      19             : 
      20    41156285 : void AP_DAL_InertialSensor::start_frame()
      21             : {
      22    41156285 :     const auto &ins = AP::ins();
      23             : 
      24    41156285 :     const log_RISH old_RISH = _RISH;
      25             : 
      26    41156285 :     _RISH.loop_rate_hz = ins.get_loop_rate_hz();
      27    41156285 :     _RISH.first_usable_gyro = ins.get_first_usable_gyro();
      28    41156285 :     _RISH.loop_delta_t = ins.get_loop_delta_t();
      29    41156285 :     _RISH.first_usable_accel = ins.get_first_usable_accel();
      30    41156285 :     _RISH.accel_count = ins.get_accel_count();
      31    41156285 :     _RISH.gyro_count = ins.get_gyro_count();
      32    41156285 :     WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);
      33             : 
      34   164625140 :     for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
      35   123468855 :         log_RISI &RISI = _RISI[i];
      36   123468855 :         const log_RISI old_RISI = RISI;
      37             : 
      38             :         // accel data
      39   123468855 :         RISI.use_accel = ins.use_accel(i);
      40   123468855 :         if (RISI.use_accel) {
      41    82388166 :             RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt);
      42             :         }
      43             : 
      44             :         // gryo data
      45   123468855 :         RISI.use_gyro = ins.use_gyro(i);
      46   123468855 :         if (RISI.use_gyro) {
      47    82388166 :             RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);
      48             :         }
      49             : 
      50   123468855 :         update_filtered(i);
      51             : 
      52   123468855 :         WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);
      53             : 
      54             :         // RISJ holds per-instance gyro bias metadata. These are constants
      55             :         // set by the backend, so the message is only written when changed.
      56   123468855 :         log_RISJ &RISJ = _RISJ[i];
      57   123468855 :         const log_RISJ old_RISJ = RISJ;
      58   123468855 :         RISJ.gyro_bias_limit = ins.get_gyro_bias_limit_rads(i);
      59   123468855 :         RISJ.gyro_bias_init_dps = ins.get_gyro_bias_init_dps(i);
      60   123468855 :         WRITE_REPLAY_BLOCK_IFCHANGED(RISJ, RISJ, old_RISJ);
      61             : 
      62             :         // update sensor position
      63   123468855 :         pos[i] = ins.get_imu_pos_offset(i);
      64             :     }
      65    41156285 : }
      66             : 
      67             : // update filtered gyro and accel
      68   123468855 : void AP_DAL_InertialSensor::update_filtered(uint8_t i)
      69             : {
      70   123468855 :     if (!is_positive(alpha)) {
      71             :         // we use a constant 10Hz for EKF filtered accel/gyro, making the EKF
      72             :         // independent of the INS filter settings
      73        2006 :         const float cutoff_hz = 10.0;
      74        2006 :         alpha = calc_lowpass_alpha_dt(get_loop_delta_t(), cutoff_hz);
      75             :     }
      76   123468855 :     if (is_positive(_RISI[i].delta_angle_dt)) {
      77    82388166 :         gyro_filtered[i] += ((_RISI[i].delta_angle/_RISI[i].delta_angle_dt) - gyro_filtered[i]) * alpha;
      78             :     }
      79   123468855 :     if (is_positive(_RISI[i].delta_velocity_dt)) {
      80    82388166 :         accel_filtered[i] += ((_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) - accel_filtered[i]) * alpha;
      81             :     }
      82   123468855 : }